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Abstract 

The  motivation  of  this  research  is  to  address  the  use  of  bearing-only  measurements 
taken  by  an  optical  sensor  to  aid  an  Inertial  Navigation  System  (INS)  whose 
accelerometers  and  gyroscopes  are  subject  to  drift  and  bias  errors.  The  concept  of 
Simultaneous  Localization  And  Mapping  (SLAM)  is  employed  in  a  bootstrapping 
manner:  the  bearing  measurements  are  used  to  geolocate  ground  features,  following 
which  the  bearings  taken  over  time  of  the  said  ground  features  are  used  to  improve  the 
navigation  state  provided  by  the  INS.  In  this  research  the  INS  aiding  action  of  tracking 
stationary,  but  unknown,  ground  features  over  time  is  evaluated.  It  does  not,  however, 
address  the  critical  image  registration  issue  associated  with  image  processing.  It  is 
assumed  that  stationary  ground  features  are  able  to  be  detected  and  tracked  as  pixel 
representations  by  a  real-time  image  processing  algorithm. 

Simulations  are  performed  which  indicate  the  potential  of  this  research.  It  is  shown 
that  during  wings  level  flight  at  constant  speed  and  fixed  altitude,  an  aircraft  that 
geolocates  and  tracks  ground  objects  can  significantly  reduce  the  error  in  two  of  its  three 
dimensions  of  flight,  relative  to  an  Earth-fixed  navigation  frame.  The  aiding  action  of 
geolocating  and  tracking  ground  features,  in-line  with  the  direction  of  flight,  with  a 
downward  facing  camera  did  not  provide  improvement  in  the  aircraft’s  x-position 
estimate.  However,  the  aircraft’s  y-position  estimate,  as  well  as  the  altitude  estimate,  were 
significantly  improved. 
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AUTOMATED  DRIFTMETER  FUSED  WITH 


INERTIAE  NAVIGATION 

I.  Introduction 

Much  work  has  been  accomplished  in  recent  years  to  reduce  the  military’s 
dependence  on  the  Global  Positioning  System  (GPS).  General  Norton  Schwartz,  the  19^* 
Chief  of  Staff  of  the  Air  Force,  was  quoted  in  January  of  2010  as  saying. 

Global  positioning  has  transformed  an  entire  universe  of  war-fighting 
capability.  Our  dependence  on  precision  navigation  in  time  will  continue  to 
grow,...  It  seemed  critical  to  me  that  the  joint  force  reduce  its  dependence  on 
GPS  aid...  Our  operations  cannot  grind  to  a  halt  for  a  degraded  or  denied 
system,...  We  must ...  proceed  to  build  more  resilient  systems...  [3] 

This  thesis  develops  a  theoretical  analysis  of  an  integrated  airborne  navigation  system 
fusing  optical  and  inertial  measurements.  The  specific  focus  is  on  autonomy  where  GPS 
signals  are  not  available,  but  precision  navigation  is  achieved  through  the  use  of  passive 
self-contained  sensors.  This  work  is  a  follow-on  to  previous  research  efforts  at  the  Air 
Force  Institute  of  Technology  (AFIT),  utilizing  a  strategy  of  fusing  bearing  measurements 
with  an  optical  sensor  and  inertial  navigation  [3],  [4],  [5].  This  research  continues  to 
investigate  the  use  of  bearing  measurements,  taken  with  a  monocular  camera,  as  a  viable 
aid  to  inertial  sensors,  in  order  to  provide  automated  navigation  comparable  to  modern 
GPS  precision. 

1.1  History 

Since  the  inception  of  the  heavier-than-air  aircraft  in  1903,  methods  and  tools  for 
navigation  have  evolved  tremendously.  Arthur  Hughes,  the  author  of  History  of  Air 
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Navigation  [6],  stated,  “in  the  first  twenty  years  of  flying,  men  were  equal  to,  or  better 
than,  the  machine”.  Navigation  tools  were  not  available  and  the  ability  to  navigate  was 
dependent  on  the  skill  and  instinct  of  the  pilot.  This  section  provides  the  historical 
background  of  a  few  of  the  early  navigation  methods  that  are  related  to  the  development  of 
this  modem  application  of  the  driftmeter  concept  fused  with  inertial  navigation. 

1.1.1  Air  Pilotage. 

Air  pilotage,  or  piloting,  is  the  method  of  directing  an  aircraft  from  place  to  place  by 
referring  to  visible  landmarks  on  Earth’s  surface,  such  as  light-beacons,  landmarks, 
railroads,  rivers,  mountains,  or  lakes,  with  the  use  of  a  map  and  dead  reckoning  [7].  Dead 
reckoning,  in  the  context  of  pilotage,  combines  the  knowledge  of  the  aircraft’s  position 
over  visible  landmarks  with  map  readings  and  the  measured  time  between  the  landmarks 
to  determine  ground  speed,  estimated  time  of  arrival,  required  heading  changes,  and/or 
wind  speed  and  direction.  There  are  many  limitations  to  navigating  solely  on  pilotage. 

The  most  obvious  being  that  it  requires  an  accurate  land  map  and  ideal  conditions  of 
flight.  For  instance,  the  weather  must  be  clear  and  the  aircraft  must  be  flying  low  enough 
so  that  the  landmarks  can  be  seen. 

1.1.2  Driftmeter. 

The  driftmeter  was  one  of  the  earliest  instruments  developed  to  determine  the  effect 
of  wind  on  the  direction  of  flight  and  the  ground  speed  of  an  aircraft.  A  dedicated 
navigator  used  information  from  an  altimeter  and  an  airspeed  indicator  along  with  bearing 
measurements  of  ground  features  to  calculate  the  ground  speed  and  the  directional  drift 
caused  by  wind  vectors  on  his  aircraft  during  flight.  The  bearing  measurements  were 
determined  by  visibly  tracking  stationary  objects  on  the  ground  or  whitecaps  on  the  water 
surface  [7].  The  drift  meter  was  installed  so  that  a  zero  degree  line  was  parallel  to  the 
longitudinal  axis  of  the  aircraft  and  the  navigator  would  have  an  unobstructed  view 
downward  through  a  glass  plate  in  the  floor  of  the  aircraft.  The  track  arm  could  be  rotated 
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so  that  objects  on  the  ground  appear  to  travel  along  the  horizontal  drift  line.  The  amount 
that  the  horizontal  drift  line  rotated  from  the  zero  degree  line  would  indicate  the  drift 
angle. 

The  Pioneer  Speed  and  Drift  Indicator  was  one  of  these  devices  [8].  It  is  illustrated 
graphically  in  Figure  1.1  and  can  be  viewed  online  at  the  Smithsonian  “Time  and 
Navigation”  webpage  [9].  It  had  a  pivoting  eye  piece  for  the  navigator  to  look  through, 
which  was  mounted  on  an  arm  that  established  a  set  distance  between  the  navigator’s  eye 
and  the  horizontal  drift  line.  (The  set  distance  between  the  eye  piece  and  the  horizontal 
drift  line  can  be  compared  to  the  focal  distance  of  a  camera.)  The  horizontal  drift  line  had 
two  sets  of  cross  wires  that  the  navigator  used  to  initiate  and  stop  a  timer  as  a  ground 
object  crossed  under.  The  ground  speed  of  the  aircraft  was  then  determined  by  dividing 
the  altitude  of  the  airplane  above  the  ground  by  the  time  required  for  the  ground  feature  to 
pass  from  one  cross  wire  to  the  next.  While  navigation  measurements  were  being  made, 
the  aircraft  would  have  to  be  flown  straight  and  level. 


Figure  1.1:  Pioneer  Drift  Meter 
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A  more  advanced  driftmeter,  named  the  B-3,  was  manufactured  by  the 
Eclipse-Pioneer  Division  of  the  Bendix  Aviation  Corp.,  and  was  standard  equipment  on 
Air  Force  and  transoceanic  aircraft  [7]  through  the  World  War  II  and  early  Cold  War  eras 
[10].  It  was  stabilized  by  a  gyroscope,  which  allowed  the  set  of  reference  timing  lines  to 
remain  horizontal  at  all  times,  regardless  of  turbulence  and  within  20  degrees  of  roll,  pitch, 
and  yaw  of  the  aircraft.  It  had  two  different  eye  pieces,  which  allowed  for  viewing  ground 
features  at  different  elevations:  one  for  normal  sight  and  one  for  three  times  magnification. 

The  drift  meter  was  a  proven  tool  for  determining  the  drift  angle,  wind  speed,  and 
ground  speed  of  an  aircraft  by  tracking  the  bearing  angle  of  distinct  ground  features.  It 
was  utilized  over  land  with  stationary  features  such  as  houses,  roads,  trees,  and  rivers.  It 
was  also  used  for  transoceanic  flights  where  the  navigator  utilized  whitecaps  as  a 
reference  point.  Although  the  driftmeter  improved  navigation  compared  to  the  method  of 
air  pilotage,  it  also  shared  several  similar  shortcomings  which  greatly  limited  navigation. 

It  required  flight  conditions  to  be  ideal  in  order  to  view  ground  objects,  which  limited  the 
altitude  of  flight  and  flight  through  or  above  cloud  formations.  Perhaps  the  greatest  of  its 
shortcoming  was  the  accrued  error,  because  the  navigator  was  not  physically  capable  of 
continuously  monitoring  changes  in  flight  conditions  (altitude,  airspeed,  etc.). 

1.1.3  Inertial  Navigation. 

The  ability  to  determine  position,  velocity,  and  attitude  through  the  use  of  inertial 
sensors  revolutionized  navigation.  This  was  first  demonstrated  by  the  Germans  during 
World  War  II  aboard  the  V-2  ballistic  missile  and  further  developed  in  the  United  States  in 
the  late  1940s  and  early  1950s  by  the  MIT  Instrumentation  Laboratory,  Northrop,  and  the 
Autonetics  division  of  North  American  Aviation,  under  Air  Force  sponsorship  [II].  By 
integrating  a  body’s  acceleration  and  angular  rotation  rate,  the  position,  velocity,  and 
attitude  could  be  determined. 
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Inertial  navigation  systems  (INS)  have  remained  a  vital  tool  in  modern  navigation. 
Reference  [12]  states  that  INSs  are  special  because  they  are  “self-contained:  they  are 
independent  of  weather  conditions  and  are  operable  anywhere  in  seas,  underwater,  lands, 
tunnels,  or  in  air.”  The  self-contained  nature  of  INS  is  the  result  of  employing  six  inertial 
sensors.  Three  orthogonally  mounted  accelerometers  are  used  to  detect  the 
three-dimensional  specific  forces  acting  on  the  body  and  three  gyroscopes  are  used  to 
establish  the  spatial  attitude  in  a  three-dimensional  Cartesian  coordinate  frame  [13].  The 
quality  of  these  inertial  sensors  directly  reflects  the  precision  and  performance  of  the  INS. 

INS’s  have  been,  and  continue  to  be,  used  in  a  variety  of  applications.  They  can  be 
found  on  ships,  aircraft,  submarines,  guided  missiles,  spacecraft,  and,  to  some  extent, 
modern  automobiles,  where  each  specific  application  requires  different  levels  of  precision. 
Grewal  [14]  defined  general  usage  categories  as  strategic  (very  high  performance 
navigation),  navigational  (medium- accuracy  navigation),  tactical  (low-accuracy 
navigation),  or  consumer  (non-navigational  applications).  The  performance  of  the  INS  is 
characterized  by  the  quality  of  the  inertial  sensors,  because  their  measurements  are 
integrated  over  time  to  determine  the  INS  output.  Therefore,  the  integration  of  small 
accelerometer  and  gyroscope  errors  over  time  cause  navigation  precision  to  deteriorate 
over  time.  Consequently,  the  quality  of  the  inertial  sensors  determines  how  long  the 
navigation  information  can  be  trusted  and,  like  anything  else,  the  price  generally 
determines  their  quality. 

1.1.4  Satellite  Based  Navigation. 

The  first  fully  operational  satellite-based  navigation  system  was  the  U.S.  Navy’s 
system  known  as  Transit,  which  was  developed  and  implemented  from  the  early  to 
mid-1960’s  [15].  Each  satellite  was  a  self-contained  navigation  beacon.  It  was  designed  to 
update  dead  reckoning  navigational  information  from  an  INS  aboard  the  Polaris 
submarines,  where  the  position  updates  were  done  at  intervals  of  more  than  an  hour.  Since 
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Transit  was  designed  primarily  for  oceanic  vessels,  accuracy  was  degraded  for  aircraft  or 
users  on  land  by  uncertainties  in  their  altitude  and  their  velocity. 

The  second  operational  navigation  satellite  system,  Cicada,  was  developed  by  Russia 
in  response  to  the  Cold  War.  It  was  similar  in  design  to  Transit  and  primarily  used  for  ship 
navigation  [15].  Two  more  satellite  navigation  programs  were  established  and  began 
development  in  the  mid  1960’s:  “621B”  by  the  Air  Force  Space  and  Missile  Organization 
(SAMSO)  and  “Timation”  by  the  Naval  Research  Laboratory  (NRL)  [15]. 

“In  1968,  the  Joint  Chiefs  of  Staff  (JCS)  issued  new  requirements  for  precisely 
locating  military  forces  worldwide”  [15].  The  most  stringent  of  these  requirements  were 
for  aircraft,  which  became  the  driving  parameters  for  a  newly  established  DOD 
Navigation  Satellite  Executive  Steering  Group  (NAVSEG).  This  group  commissioned  a 
number  of  comparative  studies  and  evaluated  three  US  satellite  navigation  systems, 
ultimately  leading  to  the  GPS  program. 

Since  the  first  launch  of  an  operational  GPS  satellite  in  1978  [15],  GPS  has  become 
the  benchmark  for  precision  navigation.  It  requires  that  a  minimum  of  four  satellites  are 
visible  to  the  user  to  determine  his  or  her  position.  The  orbital  configuration  utilized  a 
total  of  24  satellites  in  6  different  orbits,  providing  a  minimum  of  six  satellites  in  view  at 
any  time.  Unlike  the  previous  satellite  navigation  systems,  it  provided  continuous 
three-dimensional  positioning  needed  for  aircraft.  The  military  and  civilian  applications 
are  countless. 

At  the  time  [15]  was  written,  the  Standard  Positioning  Service  (SPS)  provided  to 
civilian  users  was  6  [m]  accuracy  and  the  Precise  Positioning  Service  (PPS)  provided  to 
military  users  was  2.3  [m]  accuracy.  Because  of  the  error  accumulation  over  time 
associated  with  INS,  GPS  better  satisfied  the  requirements  set  for  a  precise  navigation 
system.  However,  low  power  signals  transmitted  from  the  satellites  require  that  the  user’s 
antenna  have  a  direct  Eine  of  Sight  (EOS)  with  at  least  four  satellites.  This  prevents  usage 
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indoors  and  limits  usage  in  urban  canyons.  GPS  is  also  vulnerable  to  attack  or  disruption, 
such  as  jamming  or  spoofing  [16]. 

1.2  Motivation 

Advancements  in  computer  technology  over  the  past  few  decades  has  drastically 
increased  data  processing  speeds  and  thus  improved  the  usage  of  recursive  algorithms  like 
the  Kalman  filter,  introduced  in  1960  [2].  The  Kalman  filter  algorithm  has  been  used  to 
merge  inertial  navigational  data  from  Inertial  Measurement  Units  (IMU)  with  position 
data  from  GPS  receivers  to  create  an  accurate  passive  navigation  solution.  Passive 
navigation  systems  are  required  for  military  applications  because  they  do  not  transmit 
signals  out  for  the  purpose  of  navigating;  therefore,  they  are  not  easily  detected.  Active 
navigation  tools  using  radar  are  less  desirable  in  combat  environments. 

The  performance  of  an  INS  is  characterized  by  the  drift  of  the  accelerometers  and 
gyroscopes  discussed  in  section  1.1.3.  The  INS  accuracy  can  be  improved  through  the  use 
of  more  accurate  sensors;  however,  the  cost  of  these  sensors  quickly  escalates.  An 
alternative  approach,  referred  to  as  integrated  navigation  [17],  utilizes  an  additional 
navigation  source  along  with  a  Kalman  filter  algorithm  to  aid  the  INS  and  significantly 
reduce  its  drift  over  time.  When  this  technique  is  employed,  the  INS  is  allowed  to  drift 
without  influence  from  the  additional  navigational  source.  The  Kalman  filter  utilizes  the 
measurements  from  the  additional  navigation  source  and  estimates  the  drift  error  of  the 
INS,  which  is  then  subtracted  from  the  output  of  the  INS  to  improve  navigation 
performance.  The  INS  without  any  aiding  is  referred  to  as  the“Free  INS”. 

The  work  presented  in  this  thesis  is  motivated  by  the  possibilities  of  aiding  a 
navigation-grade  INS  with  an  optical  sensor,  acting  as  a  automated  driftmeter,  to  achieve 
precision  results  similar  to  GPS.  Therefore,  it  provides  an  alternative  passive  navigation 
system  for  military  aircraft  applications.  A  three-dimensional  measurement  equation  will 
be  utilized  in  this  work  that  blends  optical  sensor  data  of  tracked  ground  features  with 
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output  INS  data.  This  will  be  done  on  the  principles  of  the  method  developed  by  Pachter 
and  Relyea  [18]  [3]. 

Much  research  has  been  accomplished  in  recent  years  on  a  process  known  as 
Simultaneous  Localization  and  Mapping  (SLAM)  [19],  which  is  the  process  of  jointly 
establishing  an  aircraft’s  ownship  position  and  geolocating  ground  features.  For  this 
particular  work,  geolocation  is  the  act  of  acquiring  stationary  ground  features  to  establish 
reference  points,  whose  bearings  will  be  measured  over  time.  These  measurements  will  be 
used  to  aid  the  INS  in  order  to  improve  the  aircraft’s  position  estimate.  This  particular 
method  of  SLAM  is  herein  referred  to  as  “bootstrapping”.  First,  the  aircraft’s  somewhat 
inaccurate  position  and  bearing  measurement  are  used  to  geolocate  a  ground  feature. 

Then,  the  bearing  measurements  of  the  said  ground  feature  taken  over  time  are  used  to  aid 
the  aircraft’s  INS  in  order  to  improve  the  aircrafts’s  position  estimate.  These  steps  are 
taken  in  the  same  way  as  a  man  is  able  to  “pull  himself  out”  of  quicksand  by  his  own 
bootstraps. 

The  advantage  of  this  method  of  SLAM  is  that  it  allows  for  navigation  without  a 
strict,  predefined  map  or  a  priori  information  of  ground  features.  The  disadvantage  is  that 
the  aircraft’s  position  uncertainty  increases  with  time.  However,  while  a  ground  feature  is 
being  tracked  over  time,  its  position  uncertainty  does  not  increase.  It  remains  the  same 
from  the  moment  of  geolocating  that  ground  feature.  Thus,  an  improved  navigation 
system,  comparable  to  GPS,  is  provided. 

Image  processing  associated  with  the  geolocation  and  tracking  of  ground  features 
from  raw  optical  sensor  data  is  still  a  developing  field.  The  aircraft  navigators  of  the  early 
1900’s  had  the  God  given  ability  of  discernment  and  reason  as  they  selected  stationary 
ground  features  to  track  while  operating  the  driftmeter.  This  is  not  an  easy  task  for  a 
computer,  which  must  scan  a  two-dimensional  pixel  representation  of  a  three-dimensional 


8 


world  in  order  to  detect  the  best  features  to  track  and  flawlessly  perform  the  image 
registration  task  as  it  moves  from  frame  to  frame. 

Two  algorithms  currently  used  are  Scale  Invariant  Feature  Transform  (SIFT)  and 
Speeded  Up  Robust  Features  (SURF)  [20],  [21].  One  of  the  most  challenging  tasks  is  the 
process  of  determining  the  slant  range  to  a  specific  ground  feature  while  the  terrain 
elevation  is  changing.  The  absolute  altitude  was  used  for  calculating  the  ground  speed 
while  using  the  driftmeter  [7].  The  absolute  altitude,  while  flying  over  land,  was 
determined  by  subtracting  the  terrain  elevation  from  the  altitude  measurement  provided  by 
an  altimeter. 

Advantages  of  the  automated  feature  tracking  algorithm  are  that  it  would  have  the 
ability  to  track  more  than  one  ground  feature  at  a  time  and  it  would  be  able  to 
continuously  track  and  update  the  INS  at  increments  much  smaller  than  humanly  possible. 
The  navigator  of  the  past  would  only  periodically  track  features  in  order  to  determine 
ground  speed  for  dead-reckoning,  creating  accumulated  error  throughout  the  flight. 

1.3  Scope  and  Assumptions 

There  are  very  few  things  that  are  definite  in  this  world.  Even  as  you  read  these 
words,  you  may  be  very  confident  in  your  position  of  where  you  are  sitting  and  confident 
in  the  distance  of  stationary  objects  around  you,  but  your  exact  position  is  only  stationary 
relative  to  Earth.  Calculating  your  position  or  distance  from  stellar  objects,  not  rotating 
with  the  Earth,  is  much  more  difficult.  Moreover,  there  is  a  certain  amount  of  doubt  in  your 
position.  This  doubt  or  uncertainty  is  quantified  by  the  term  covariance,  from  statistics. 

The  contribution  of  this  research  is  the  calculation  of  the  covariance  associated  with 
geolocating  a  ground  feature  and  then  tracking  that  feature  until  it  disappears  from  the 
field  of  view  (EOV)  of  the  camera,  whereupon  is  replaced  by  a  newly  geolocated  ground 
feature.  This  process  has  been  accomplished  by  other  research,  but  not  in  the  particular 
manner,  involving  bearing  measurements,  presented  in  this  research. 
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To  evaluate  the  possible  contributions  of  bearing  measurements  as  a  viable  aid  to  an 
INS,  a  plausible  navigation  scenario  is  established.  The  scope  of  this  research  focuses  on 
the  navigation  contribution  of  SLAM.  Consequently,  assumptions  are  made  in  the 
scenario  which  improve  the  focus  of  this  research.  The  greatest  of  these  assumptions  is 
that  stationary  ground  features  may  be  detected  and  tracked  as  pixel  representations  by  a 
real-time  image  processing  algorithm.  The  next  is  that  two  distinct  ground  features  are 
available  for  each  image  frame  throughout  the  navigation  scenario  and  their  elevation  is 
known  to  be  zero.  Involving  the  navigation  environment,  it  is  assumed  that  the  aircraft  is 
nominally  flying  straight  and  level  at  a  constant  ground  speed  over  a  flat  and  non-rotating 
Earth. 

The  critical  image  registration  issue  associated  with  image  processing  is  outside  the 
scope  of  this  research  and  will  not  be  addressed.  It  is  fair  to  assume  that  ground  features 
may  be  detected  and  tracked  as  pixel  representations,  because  current  image  processing 
algorithms  identify  features  as  pixel  locations  on  a  coordinate  system  relative  to  each 
image  frame  [16].  However,  it  was  stated  in  2008  that  a  “great  leap  in  feature 
generation/tracking  technology  and  significantly  more  precise  optics”  are  required  for  the 
development  of  a  viable  vision  based  navigation  platform  [19]. 

The  assumption  that  two  distinct  ground  features  are  available  does  not  imply  that 
these  features  are  known.  When  utilizing  the  navigation  method  of  pilotage  the  ground 
features  were  associated  with  maps  that  identified  specific,  known  feature  locations.  This 
research  considers  features  which  are  measured  with  an  uncertainty  (covariance)  relative 
to  the  quality  (resolution)  of  the  optical  sensor.  However,  their  locations  are  not  known. 
The  assumption  that  the  ground  feature  elevations  are  known/zero  is  fair  because 
generally  ground  feature  heights  are  relatively  small  in  comparison  with  the  elevation  of 
aircraft  flying  overhead. 
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To  ensure  a  proper  evaluation  of  the  benefit  of  bearings  tracking  to  aid  an  INS,  the 
calculations  of  this  navigation  scenario  are  justifiably  simplified,  with  the  assumption  of  a 
fiat  and  non-rotating  Earth.  By  simplifying  these  parameters,  the  flight  dynamics  are 
simplified.  Furthermore,  the  development  of  errors  from  the  Free  INS  and  the  Kalman 
filter’s  estimate  of  those  errors  are  more  easily  compared  with  the  truth  data  in  the 
simulations. 

1.4  Related  Research 

This  section  presents  modern  research  in  the  field  of  navigation  by  use  of  optical 
sensors.  It  presents  their  research  task  along  with  the  specifications  of  equipment  used. 
Finally,  their  achieved  navigation  precision  and  the  research’s  limitations  are  presented. 

1.4.1  Binocular  Feature  Tracking  Fused  with  Inertial  Measurements. 

Veth’s  AFIT  PhD  dissertation  in  2005  was  inspired  by  the  precision  navigation 
capabilities  of  animals/biometrics  [16].  It  claimed  that  “there  exists  a  natural  synergy 
between  imaging  and  inertial  systems”  [16].  Veth  performed  indoor  measurements  and 
outdoor  flight  test  with  two  identical  monochrome  digital  cameras,  a  consumer-grade 
strapdown  IMU,  and  a  tactical-grade  strapdown  IMU.  His  system  was  configured  such 
that  both  IMUs  would  record  measurements  simultaneously.  This  configuration  insured 
the  same  dynamics  for  both  grades  of  IMUs  and  thus  fair  testing  between  the  two.  The 
specifications  for  his  two  IMUs  are  listed  in  Table  1.1. 

The  cameras  had  a  1280  x  1024  (1.3  megapixel  (MP))  resolution.  They  were  rigidly 
mounted  with  space  between  them,  allowing  distance  for  triangulation  for  range 
calculations  of  the  features.  They  had  a  measurement  rate  of  three  frames  per  second  at 
full  resolution.  The  camera  data  was  processed  using  the  SIFT  algorithm,  which 
determined  features  in  each  frame  and  tracked  them  according  to  their  pixel  location. 


11 


Table  1.1:  Veth’s  Inertial  Measurement  Sensor  Specifications  for  the  Crista 
consumer-grade  IMU  and  the  Honeywell  HG1700  tactical-grade  IMU.  Veth 
estimated  the  parameters  with  an  asterisk  since  they  were  not  included  in 
manufacturer  specification  data  [16]. 


Parameter 

UNITS 

Crista  IMU 

HGI700 

Sampling  interval 

ms 

5.0 

lO.O 

Gyro  bias  sigma 

deg/hr 

1800 

1.0 

Gyro  bias  time  constant 

hr 

2* 

2* 

Angular  random  walk 

deg/ 

2.23 

0.3 

Gyro  scalefactor  sigma 

PPM 

10000 

150 

Accel  bias  sigma 

mjs^ 

0.196 

0.0098 

Accel  bias  time  constant 

hr 

2* 

2* 

Velocity  random  walk 

m/s  !  ^fhr 

0.261 

0.57* 

Accel  scalefactor  sigma 

PPM 

10000 

300 

An  extended  Kalman  filter  was  used  to  aid  the  SIFT  algorithm  according  to  matched 
estimations  of  propagated  feature  locations,  determining  whether  they  were  good  or  bad 
features.  As  new  features  were  determined  to  be  stronger,  they  replaced  weaker  ones.  His 
algorithm  assumed  that  the  navigation  state  errors  and  the  landmark  errors  were 
independent.  His  system  “learned”  its  environment  by  maintaining  a  database  of  the 
tracked  features  and  position  estimates. 

Veth  obtained  meter  level  position  precision  in  the  indoor  environment,  but  the  flight 
test  results  were  non-conclusive,  because  the  test  lacked  a  faithful  truth  source. 
Ultimately,  his  work  proved  that  a  combination  of  optical  data  and  inertial  data  can 
provide  precision  navigation  capabilities  equivalent  to  GPS,  while  utilizing  binocular 
measurement  geometry  to  determine  the  LOS  range  of  tracked  features. 
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1.4.2  Covariance  Analysis  of  Vision  Aided  Navigation  by  Bootstrapping. 

Relyea’s  AFIT  Master’s  thesis  in  2012  followed  the  work  of  Guner  Mutlu,  a  previous 
AFIT  student  under  the  guidance  of  Dr.  Meir  Pachter  [3].  Relyea  further  developed  the 
calculations  for  bearing  measurements  of  ground  features  from  a  single/monocular 
camera,  ultimately  used  in  this  current  work.  He  also  developed  the  simplified  flight 
dynamics,  used  herein,  where  he  applied  the  camera  measurements  to  a  Free  INS  and 
performed  covariance  analysis.  His  Free  INS  specifications  were  calculated  based  on  1 
propagated  sensor  bias  errors,  which  qualifies  it  as  a  navigation-grade  INS.  The  accuracy 
of  his  camera  bearing  measurements  was  based  on  an  assumption  of  a  9  MP  camera  with 
an  aspect  ratio  of  one.  His  non-dimensional  INS  specifications  are  listed  in  Table  1.2. 

Table  1.2:  Relyea’s  calculated  l-cr  inertial  sensor  specifications  provided  a 
variance  of  1  ^  drift  error  due  to  the  influence  of  biases.  These  values  are 

nr 

non-dimensional  according  to  his  navigation  scenario  [3]. 


Parameter  UNITS 

1  km/hr 

Sampling  interval  ms 

10 

Gyro  bias  sigma  Non-Dim 

1.0912  X  10-5 

Accel  bias  sigma  Non-Dim 

9.0935  X  10-^ 

His  camera  only  tracked  two  ground  features  at  a  time,  and  he  started  the  navigation 
scenario  with  two  “known”  ground  features  and  progressed  until  he  only  tracked 
“unknown”  features.  The  covariance  of  newly  tracked  ground  feature’s  (x  and  y)  position 
was  determined  by  adding  the  covariance  of  the  navigation  state  error  (x  and  y)  position, 
at  the  time  of  geolocating  the  new  ground  feature,  with  the  covariance  of  the  camera 
measurement. 

The  resulting  standard  deviations  of  his  covariance  analysis  are  provided  in  Table  1.3. 
Relyea’s  l-cr  standard  deviation  achieved  meter  level  precision  with  the  simulated  flight  of 
one  hour,  while  utilizing  INS  specifications  equivalent  to  a  navigation- grade  INS. 
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However,  he  only  used  INS  sensor  bias  in  his  system  -  not  process  noise,  a.k.a  sensor 
drift.  Also,  he  only  evaluated  the  overall  system  covariance  -  not  the  aided  results. 


Table  1.3:  Relyea’s  covariance  analysis  results  of  one  hour  simulated  flight  [3]. 


Std  Dev 

Unaided  Final  Value 

Aided  Peak  Value 

Aided  Final  Value 

(Tx 

1  km 

4.7  m 

4.7  m 

O-y 

1  km 

4.0  m 

4.0  m 

CTz 

0.7071  km 

0.10  m 

0.0389  m 

7  X  10“^  m/s 

8.21  X  10-5  m/s 

3.93  X  10-5  m/s 

(Tv, 

7  X  10“^  m/s 

3.06  X  10-5  m/s 

2.29  X  10-5  m/s 

4  X  10“^  m/s 

3.9  X  10-5  m/s 

2.16X  lO-^s 

(T  (f, 

3.27  X  10-5  rad 

2.40  X  10-^  rad 

1.07  X  10-’  rad 

(Te 

3.27  X  10-5  rad 

3.16  X  10-6  rad 

2.39  X  10-’  rad 

(T ,/, 

3.27  X  10-5  rad 

2.23  X  10-5  rad 

2.23  X  10-5  rad 

1.4.3  Inertial  Navigation  System  Aiding  Using  Vision. 

Quarmyne’s  AFIT  Master’s  thesis  in  2013  followed  the  work  of  Relyea  [4].  His  goal 
was  to  apply  a  linear  Kalman  filter  to  the  covariance  analysis  of  the  previously  developed 
three-dimensional  flight  simulation.  The  camera  resolution  and  inertial  sensor 
specifications  were  the  same  as  those  of  Relyea  with  only  bias  errors  applied  to  the  Free 
INS. 

Quarmyne  also  began  his  navigation  scenario  with  two  “known”  ground  features  and 
progressed  until  he  only  tracked  “unknown”  features.  However,  advancements  were  made 
in  his  research  with  derivations  of  newly  tracked  ground  feature’s  geolocation  and 
associated  error  covariance  by  using  the  corrected  (aided)  measurement  equation.  This 
method  is  further  developed  in  this  current  research. 
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The  standard  deviations  derived  from  the  covariance  of  his  Kalman  filter  final  values 


are  presented  in  Table  1.4.  Quarmyne’s  Kalman  filter  estimated  the  error  of  the  Free  INS. 
By  correcting  that  error,  an  improvement  of  one  order  of  magnitude  was  achieved. 

Table  1 .4:  Qyarmyne’s  Kalman  filter  results  after  one  hour  of  simulated  flight. 

The  ’’Unaided  Final  Values”  are  the  results  of  his  Free  INS  and  the  ’’Aided 
Peak  Values”  are  the  results  of  subtracting  the  KF  estimated  values  from  the 
corresponding  Free  INS  values  [4]. 


Std  Dev 

Unaided  Final  Value 

Aided  Final  Value 

CTx 

6.56  km 

692.72  m 

(Ty 

6.56  km 

18.84  m 

(^z 

0.707  km 

5.72  m 

3.67  X  10-2  m/s 

5.74  X  10-2  m/s 

O-v, 

3.67  X  10-2  m/s 

8.66  X  10-2  m/s 

3.93  X  10-2  m/s 

3.18  X  10-2  m/s 

O"  if) 

1.05  X  10-^  rad 

1.09  X  10-2  rad 

O-g 

1.05  X  10-^  rad 

3.27  X  10-2  rad 

O'  i/r 

1.05  X  10-4  rad 

5.85  X  10-2  rad 

1.5  Approach/Methodology 

The  current  research  follows  the  work  of  Quarmyne  and  utilizes  the  same  flight 
dynamics  for  a  one  hour  flight.  The  tactical-grade  Free  INS  specifications  are  developed 
with  both  sensor  bias  and  sensor  drift  errors,  equally  responsible  for  1  ^  navigation  error. 
The  automated  driftmeter  measurement  equations  are  borrowed  from  the  work  of  Relyea 
and  utilize  a  downward  facing  monocular  camera  for  optical  measurements.  A  recursive 
linear  Kalman  filter  algorithm  utilizes  Free  INS  measurements  and  automated  driftmeter 
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measurements  to  determine  an  optimal  estimate  of  the  Free  INS  error.  The  estimate  is  then 
subtracted  from  the  Free  INS  output,  providing  an  improved  navigation  state. 

Prior  to  the  beginning  of  the  navigation  scenario,  flight  conditions  include  an 
elevation  of  1000  [m]  flying  straight  and  level  over  a  flat  non-rotating  Earth,  where  a  GPS 
aided  INS  provides  precise  position  data.  At  the  beginning  of  the  navigation  scenario, 

GPS  is  no  longer  available,  the  INS  is  perfectly  aligned,  and  two  “unknown”  ground 
features  are  geolocated.  During  the  geolocation  of  all  newly  tracked  ground  features,  their 
associated  geolocation  error  estimate  is  set  equal  to  zero,  because  it  will  on  average  be 
zero.  Their  covariance  is  derived  from  the  improved  navigation  state  error  covariance  plus 
the  camera  measurement  covariance. 

1.6  Thesis  Organization 

Chapter  1  discusses  the  viability  of  vision/bearing  measurements  as  an  aid  for  inertial 
sensors  and  then  follows  up  with  the  current  state  of  related  research.  Chapter  2  provides 
the  mathematical  concepts  needed  for  the  development  of  the  modem  driftmeter  system. 
Chapter  3  introduces  the  methodology  used  to  simulate  the  system.  Chapter  4  discusses 
and  analyzes  the  results  of  the  simulations.  Chapter  5  provides  conclusions  from  the 
research  and  details  possible  future  work. 
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II.  Mathematical  Background 


This  chapter  describes  the  mathematics  and  physics  required  to  understand  inertial 
navigation,  Kalman  filtering,  and  bearings-only  ground  feature  tracking.  The  three 
reference  frames  used  in  this  work  are  presented,  followed  up  by  the  calculations  required 
to  transition  between  them.  The  concepts  of  a  strapdown  INS,  as  used  in  this  work,  are 
presented.  Design  considerations  and  the  equations  for  linear  Kalman  filtering  are 
presented.  Lastly,  the  optics  model  and  required  calculations  for  bearings-only  tracking 
are  presented.  The  bulk  of  this  discussion  is  drawn  from  the  work  of  Titterton  and  Weston 
on  strapdown  inertial  navigation  [17],  Maybeck  on  Kalman  filter  design  [1]  with  the  use 
of  Brown  and  Hwang’s  Kalman  filtering  equation  notations  [2],  and  Relyea  on  bearing 
feature  tracking  [3]. 

2.1  Reference  Frames 

For  navigation  over  the  Earth,  it  is  necessary  to  define  a  convenient  set  of  axes.  The 
inertial  frame.  Earth  frame,  navigation  frame,  wander  azimuth  frame,  and  the  body  frame 
are  the  co-ordinate  frames  described  in  [17].  Eor  this  research,  the  two  frames  of  concern 
are  the  navigation  frame  and  the  body  frame.  The  focal  frame  is  also  introduced  as  a 
concept,  but  is  not  of  major  concern  because  it  is  perfectly  aligned  with  the  body  frame. 

2.1.1  Navigation  reference  frame. 

The  navigation  frame  is  a  local  geographic  frame,  which  has  its  origin  placed  at  the 
position  of  the  aircraft  near  the  surface  of  the  Earth  (at  the  location  of  the  INS)  [17].  The 
Xn-yn  plane  is  tangent  to  the  surface  of  the  Earth,  with  the  positive  v„-axis  pointing  true 
North,  the  positive  y„-axis  pointing  East,  and  the  positive  z„-axis  pointing  down,  toward 
the  center  of  the  Earth.  This  is  referred  to  as  the  North-East-Down  (NED)  axes 
convention.  See  Eigure  2.1  for  an  illustration  of  the  navigation  reference  frame. 
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Figure  2.1:  Navigation  reference  frame 


2.1.2  Body-fixed  reference  frame. 

The  body  frame  is  affixed  to  the  aircraft,  where  the  origin  is  commonly  located  at  the 
aircraft’s  center  of  gravity.  The  axes  of  the  frame  have  the  same  rotational  motion  as  the 
body  does.  The  positive  x^-axis  points  out  of  the  nose  of  the  aircraft  creating  the  roll  axis, 
the  positive  y^-axis  points  out  the  starboard  (right)  wing  creating  the  pitch  axis,  and  the 
z/,-axis  points  out  the  bottom  of  the  aircraft,  which  creates  the  yaw  axis.  See  Figure  2.2  for 
an  illustration  of  the  aircraft  body  frame. 


Figure  2.2:  Aircraft  body  frame 
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2.1.3  Camera-focal  reference  frame. 

The  focal  frame  is  rigidly  attached  to  the  camera,  with  its  origin  at  the  camera’s 
optical  center  [16].  The  Xf  and  y/  axes  point  up  and  to  the  right,  respectively,  while  the  Zf 
axis  is  perpendicular  to  the  focal  plane,  as  shown  in  Figure  2.3. 


The  focal  plane  is  best  represented  as  an  array  of  pixels  with  a  physical  height  of  H 
and  width  of  VT  [16].  The  aspect  ratio  is  determined  by  dividing  the  H  by  W.  For  this 
research,  the  FOV  captured  in  this  array  is  square  and  thus  the  aspect  ratio  is  equal  to  one. 
The  camera  is  facing  downward  and  co-located  with  the  INS;  hence,  the  Xf  axis  is  aligned 
with  the  Xb  axis  and  y/  axis  is  aligned  with  y^  axis.  The  number  of  pixels  determines  the 
image  resolution,  which  determines  the  variance  of  the  camera  measurements. 


H 


Figure  2.4:  Camera  focal  plane  represented  as  a  pixel  array 


w 
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2.2  Coordinate  System  Transformation 

This  section  develops  the  mathematics  needed  to  transition  navigation  vectors  from 
one  coordinate  frame  to  another.  Reference  [17]  is  primarily  used  for  this  introduction  of 
Euler  rotation  angles  and  the  direction  cosine  matrix  (DCM).  Although  there  are  other 
methods  of  coordinate  transformation,  this  method  is  preferred  when  dealing  with  a  stable 
platform  INS. 

The  three  orthogonal  gyroscopes  described  in  Section  1.1.3  measure  the  rotation  rate 
about  the  three  body  frame  axes.  The  corresponding  body  angles  are  referred  to  as  the 
Euler  rotation  angles.  Using  the  Euler  angles,  transformation  from  the  body  frame  to  the 
navigation  frame  can  be  carried  out  as  three  successive  rotations  about  the  three  body 
frame  axes  as  follows  [17]: 

cos  if/  -sin  i/f  0 
rotation  i/r  about  Zfe-axis,  Ci  =  sin  i/r  cos  i/r  0 

0  0  1 

cos  9  0  sin  0 

rotation  0  about  jfo-axis,  €2=  0  10 

-sin  6  0  cos  6 

1  0  0 

rotation  0  about  Xb-axis,  C3  =  0  cos  (p  -sin  (p 

0  sin  (p  cos  <p 

Thus,  the  transformation  from  the  body  frame  to  the  navigation  frame  may  be  expressed 
as  the  product  of  these  three  separate  transformations  as  follows  [17]: 

C]]  =  Cl  •  C2  •  C3 
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where  is  the  DCM  specifically  for  the  transformation  from  the  body  frame  to  the 
navigation  frame.  The  resultant  product  is  as  follows: 


C 


n  _ 

b 


cos  6  ■  cos  i/r 

cos  0  •  sin  i/r 
-sin  6 


-cos  0  •  sin  ^ 

+  sin  0  •  sin  6  ■  cos  if/ 
cos  <p  •  cos  if/ 

+  sin  0  •  sin  6  ■  sin  if/ 
sin  6  •  cos  6 


sin  (p  ■  sin  iff 

+  cos  (j)  -sinQ  •  cos  if/ 
-sin  (f)  •  cos  if/ 

+  cos  (p  ■  sinO  ■  sin  if/ 
cos  (p  •  cos  6 


(2.1) 


According  to  [17],  when  considering  small  angle  rotations,  valid  in  this  research,  the  body 


to  navigation  frame  transformation  DCM  is  further  simplified  as  follows: 


sin  0  ^  (p  cos  (p 

sin  0  ^  6  cos  6 

sin  if/  if/  cos  if/ 

Therefore,  making  these  substitutions  in  (2.1)  and  ignoring  products  of  small  angles,  the 
DCM  expressed  in  Euler  rotations  reduces  approximately  to  the  skew  symmetric  form  of 
Cl  as  follows: 

1  -if/  e 


f^n 


ip  1  -(p 


(2.2) 


[-e  cp  1  j 

If  transformation  from  the  navigation  frame  to  the  body  frame  is  desired,  then  Cl 
would  be  calculated  as  follows 


where  the  superscript  ^  indicates  that  it  is  the  transpose  of  the  matrix  being  evaluated. 


Applying  the  transpose  and  the  small  angle  rotations  provides  the  DCM  for  transforming 
from  the  navigation  to  the  body  frame,  as  follows: 


C 


b 

n 


1  if/  -e 

-ip  1  (p 


e  -(p  1 


(2.3) 
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Due  to  the  placement  of  the  camera,  the  focal  frame  is  perfectly  aligned  with  the  body 
frame,  so  the  transformation  is  simply  =  I. 


2.3  Strapdown  INS 

This  section  presents  fundamental  information  pertaining  to  the  understanding  of  the 
INS  utilized  in  this  research.  Original  applications  of  inertial  navigation  technology  used 
stable  platform  techniques  [17].  These  systems  isolated  the  rotational  motion  of  the 
vehicle  by  mounting  the  inertial  sensors  on  a  stable  platform.  Modern  systems,  including 
the  system  used  in  this  research,  have  removed  the  mechanical  complexity  of  stable 
platform  systems  by  rigidly  mounting  or  “strapping  down”  the  inertial  sensors  directly  to 
the  body  of  the  aircraft.  These  systems  have  decreased  both  cost  and  size  and  have  greater 
reliability.  However,  they  demand  increased  computational  complexity  and  the  gyros  must 
have  an  expanded  dynamics  range.  Fortunately,  both  of  these  concerns  are  easily  satisfied 
with  today’s  ring  laser  gyro  (RLG)  or  fiber  optic  gyroscope  (FOG)  technology. 

2.3.1  Specific  Force  Model. 

The  strapdown  INS  utilizes  three  orthogonally  mounted  accelerometers  whose  input 
axes  are  aligned  with  the  body  axes.  This  determines  the  specific  force  applied  to  the  body 
in  three  dimensions.  The  measured  components  of  specific  force  and  estimate  of  gravity 
are  summed  to  determine  components  of  acceleration  with  respect  to  the  navigation 
frame.  The  summed  value  is  then  integrated  once  to  determine  the  velocity  of  the  body 
and  a  second  time  to  determine  the  position  of  the  body  relative  to  the  navigation  frame. 

The  acceleration  components  of  the  aircraft  body  in  the  navigation  frame  is 
mathematically  represented,  as  follows: 


a 


(«)  _ 


j2r(n) 

df- 


(2.4) 


where  r  is  the  vector  which  describes  the  three  dimensional  position  of  the  body  in  the 
navigation  frame.  Each  accelerometer  contributes  to  the  specific  force  vector  which  is 
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described  mathematically  as  a  perfect  sensor  with  no  errors,  as  follows 

f  (")  = 

where  is  the  specific  force  due  to  gravity.  During  constant  altitude  and  wings  level 

flight,  =  a,  fy'^^  =  0,  and  =  g,  where  g  is  the  acceleration  of  gravity  and  a  is  the 

longitudinal  acceleration  of  the  aircraft  [3].  Thus, 

a  0  a 

=0-0  =  0 
0  -g  g 

and  the  skew  symmetric  [13]  matrix  form  of  the  vector  is  defined  by 

j(«)y  _  pW 
0  -g  0 

g  0  -a  (2.5) 

0  a  0 

The  skew  symmetric  matrix  of  the  specific  force  vector  is  used  in  the  development  of  the 
INS  error  equations. 

2.3.2  Imperfect  Sensors. 

The  reality  of  the  accelerometers  and  gyroscopes  is  that  they  provide  imperfect 
representations  of  the  true  specific  force  and  true  body  rotation  rate  information.  The  two 
dominant  sources  of  error  are  described  as  sensor  bias  and  sensor  random- walk  (drift). 

The  sensor  bias  error  is  random  on  start-up  but  constant  thereafter.  A  random-walk  error  is 
an  accumulation  of  zero-mean  random  errors  associated  with  “electronic  noise  from 
power  supplies,  intrinsic  noise  from  semiconductor  devices,  or  from  quantization  errors  in 
digitization”  [14].  This  research  analyzes  the  effect  of  modeling  both  of  these  errors 
separately,  as  well  as  combined. 
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2.3.3  Navigation  State. 

The  navigation  state  vector,  x,  presented  in  [3]  is  used  for  this  research.  It  is 
comprised  of  nine  states,  which  describe  the  position,  velocity,  and  attitude  of  the  aircraft 
body  in  the  navigation  frame.  The  previously  described  imperfections  from  the  sensor 
measurements  contribute  to  the  output  of  the  INS  and  are  described  as  the  error  states,  dx, 
of  the  INS.  Therefore,  the  calculated  (output)  states  of  the  INS  are  as  follows: 

Xc  =  X  +  dx 

The  error  states  are  the  primary  focus  of  this  research,  as  the  goal  is  to  evaluate  the 
contribution  of  an  automated  driftmeter’s  ability  to  help  estimate  these  INS  error  states  so 
they  can  be  removed  and  thus  provide  an  improved  navigation  state. 

2.4  Kalman  Filtering 

This  section  provides  some  basic  Kalman  filter  (KF)  design  considerations  and  the 
basic  equations  for  the  KF  loop.  Most  information  in  this  section  will  be  drawn  from  [1] 
and  [2].  Kalman  filtering  is  a  perfect  match  for  the  performance  characteristics  of  a 
navigation-grade  INS.  INSs  provide  very  good  high  frequency  information.  However,  the 
long  term,  or  low  frequency,  performance  is  poor  [1].  All  INSs  have  position  errors  that 
grow  slowly  with  time,  and  ultimately  these  errors  are  unbounded.  The  KF  utilizes  the 
statistical  characteristics  of  the  errors  in  both  the  INS  and  the  external  source  to  optimally 
estimate  the  error  from  the  INS.  This  is  used  to  reduce  error. 

2.4.1  Kalman  filter  system  design. 

The  KF  design  is  determined  by  the  application.  Maybeck  [1]  stated  that  four  design 
choices  are  available:  “total  state  space”  versus  “error  state  space”  formulation,  and 
“feedforward”  versus  “feedback”  mechanization.  These  design  aspects  are  presented  in 
Figure  2.5  with  the  aspects  utilized  in  this  research  emphasized  with  a  gray  background. 
This  is  followed  with  a  discussion  of  the  advantages  and  disadvantages  of  each. 
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Figure  2.5:  Kalman  filter  design  aspeets  [1] 


The  “total  state  spaee”  configuration  integrates  the  KF  inside  the  INS  loop,  where  the 
accelerometers,  gyroscopes,  and  external  navigation  measurements  are  the  KF  input  and 
the  navigation  state  information  is  the  output.  This  provides  “optimal  time-varying  gains,” 
thus  improving  the  performance  response  time  [1].  The  major  drawback  is  that  it  requires 
very  high  bandwidth,  which  creates  a  high  computation  burden.  Another  drawback  is  that 
the  INS  cannot  operate  without  the  filter.  The  risk  of  losing  INS  operation  completely 
makes  this  a  less  desirable  configuration. 

The  “error  state  space”  KF  is  what  is  used  in  this  current  work.  The  inputs  for  this 
KF  are  the  INS  navigation  states  and  the  external  navigation  data,  while  the  outputs  are 
the  navigation  state  error  estimates.  The  KF  dynamics  are  based  on  the  INS  error 
propagation  equations,  which  for  a  navigation-grade  INS  are  adequately  represented  as 
linear.  The  update  rate  can  be  much  lower  than  the  previously  discussed  configuration. 
Maybeck  states  that  an  effective  sampling  rate  could  be  “on  the  order  of  half  a  minute”, 
because  the  Scheuler  period  is  84  minutes  [1].  The  configuration  used  in  this  research  is  a 
variation  of  this  “error  state  space”  KF,  focusing  solely  on  the  error  states  of  the  INS  and 
utilizing  a  sampling  frequency  of  1  second. 

Furthermore,  the  “indirect  feedforward”  configuration  is  used  in  this  work.  The  basic 
block  diagram  presented  in  [1]  is  illustrated  in  Figure  2.6.  The  KF  compares  the  data  from 
each  navigational  source  and  estimates  the  errors  in  the  INS.  These  errors  are  then 
subtracted  from  the  Free  INS-provided  navigation  state,  yielding  an  improved  estimate  of 
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the  aircraft  position,  velocity,  and  attitude.  In  the  event  of  KF  or  external  navigation 
system  failure,  the  Free  INS  data  is  still  available;  hence  the  advantage  of  this 
configuration.  However,  allowing  the  INS  to  drift  with  unbounded  errors  may  compromise 
the  validity  of  the  linear  error  dynamics  model,  which  is  directly  related  to  the  KF 
performance. 


An  alternative  design  is  the  “indirect  feedback”  configuration,  illustrated  in 
Figure  2.7.  The  KF  estimates  the  INS  errors,  which  are  then  fed  back  into  the  INS  to  reset 
it.  This  feedback  action  prevents  the  unbounded  runaway  of  the  INS  error.  Therefore,  the 
“adequacy  of  a  linear  model  is  enhanced,”  improving  KF  performance  [1].  Maybeck 
further  stated  that  the  “slow  sample  rate  and  the  slow  INS  error  dynamics”  would  allow 
for  detection  of  filter  or  external  aid  failure  and  “the  correction  to  the  INS  could  be 
removed  before  much  (any)  performance  deterioration  were  caused”  [1].  This  will  be  a 
design  consideration  for  future  work  discussed  in  Chapter  5. 

2.4.2  Continuous-Time  Linear  Stochastic  System  Model. 

The  continuous-time  linear  stochastic  system  model  presented  in  [1]  and  [19]  is 

x  =  F  -  x-l-B-u-i-G-w  (2.6) 
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Figure  2.7:  Indirect  feedback  Kalman  filter  [1] 


where  x  is  the  state  vector,  u  is  a  deterministic  control  input  vector,  F  is  the  system 
dynamics  matrix,  B  is  a  deterministic  input  matrix,  G  is  the  noise  input  matrix,  and  w  is  a 
vector  of  white,  Gaussian,  noise,  w  is  described  as  a  zero-mean  Gaussian  process  with 
statistics,  that  follow: 

E{w(0  •  -I-  t)}  =  Q(0  •  S(t) 

where  Q  is  the  noise  intensity,  and  6(t)  is  the  Dirac  delta  function. 

The  continuous-time  linear  error  equation  6x,  presented  in  state  space  form  by  [3] 
and  [4],  is  as  follows: 

dx  =  A  •  dx  -I-  r  •  du  (2.7) 

where  dx  is  the  navigation  state  error  vector,  du  is  the  random  bias  vector,  A  is  the  system 
error  dynamics  matrix,  and  F  is  the  input  matrix  for  the  sensor  bias  terms.  These  error 
equations  do  not  have  a  stochastic  term.  Therefore,  they  only  model  errors  caused  by  the 
sensor  biases,  which  are  random  on  start-up  but  constant  thereafter. 

2.4.3  Linear  Measurement  Model. 

The  measurements  z  are  provided  to  the  KF  in  discrete  time  increments.  The 
equation  is  presented  in  [1]  as  follows: 

ziU)  =  H(tO  •  X(t;)  -t  V(t;) 
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where  z(?,)  is  a  vector  of  measurements  taken  at  time  instant  t,.  H(t,)  is  the  observation 
matrix,  which  relates  the  state  values  in  x  at  time  U  to  the  measurement  values  in  z 
produced  by  the  sensor  at  time  t,  .  The  covariance  of  the  white  Gaussian  noise  vector  v  is 
specified  as  follows: 

E{v(t;)  •  V^(ty)}  =  R(ti)  ■  6ij 

where  R  is  the  noise  strength  in  the  measurements  z  and  is  a  Kroeneker  delta  function 
[19]. 

2.4.4  Discrete-Time  Linear  Stochastic  System  Model. 

The  discrete-time  linear  stochastic  system  model  is  presented  in  [1]  as  follows: 

x{ti)  =  t;_i)  •  x(t,_i)  -h  Rdih-i)  •  u(t,_i)  -t  w(t,_i)  (2.8) 

where  the  state  vector  x  is  calculated  for  time  instant  (t,)  with  state  transition  matrix  O  and 
discrete  input  matrix  plus  white  Gaussian  noise  at  time  instant  Reference  [2] 
utilizes  subscript  t  for  the  discrete  time  notation,  so  (2.8)  would  instead  be  presented  as 

X^+I  =  •  X;t  +  B4  -Uk  +  Wk  (2.9) 

This  discrete-time  notation  is  adopted  herein. 

2.4.5  Discrete-Time  Kalman  Filter  Loop. 

The  recursive  equations  for  the  discrete-time  KF  loop,  as  presented  in  [2],  are 
illustrated  in  Figure  2.8.  The  recursive  loop  begins  with  an  estimate  of  the  initial 
conditions  which  are  considered  suboptimal  and  designated  with  a  superscript  minus  sign. 
The  Kalman  gain  is  then  calculated  based  on  the  initial  covariance  Pq  and  the 
measurement  covariance  Rk-  This  gain  value  is  then  used  to  update  (optimize)  the  initial 
conditions  estimate  with  initial  conditions  of  the  external  measurement  zq.  Since  this  is 
now  an  optimal  estimate,  it  is  used  by  the  system  and  the  associated  covariance  is 
calculated.  The  state  transition  matrix  O  is  then  used  to  project  ahead  (propagate)  what 
the  estimated  values  of  the  navigation  state  error  should  be  for  the  next  instant  in  time. 
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k+  Thus,  a  suboptimal  state  estimate  and  an  assoeiated  eovarianee  are  ereated.  From 
this  point,  the  reeursive  loop  starts  over  by  ealeulating  the  Kalman  gain  assoeiated  with 
the  suboptimal  estimates. 


Figure  2.8:  Linear  Kalman  Filter  Reeursive  Loop  [2] 


2.5  Camera  Model 

This  seetion  introduees  the  optieal  sensor  model.  The  ground  feature  is  a  pixel  in  the 
eamera’s  foeal  plane.  It  explains  the  ealeulations  of  the  ground  feature’s  bearing 
measurements  from  the  pixel’s  position  in  the  eamera’s  foeal  plane.  The  bearing 
measurements  are  used  to  develop  the  measurement  equation  for  the  Kalman  filter  and  are 
borrowed  from  [5]  and  [3]. 
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2.5.1  Optical  Sensor  Model. 

The  measurements  that  are  used  to  aid  the  Free  INS  are  from  an  optieal 
sensor/eamera  located  on  the  belly  of  an  aircraft,  co-located  with  the  INS,  and  pointing 
downward  in  the  same  manner  as  a  driftmeter.  The  optical  sensor  is  modeled  as  a  pinhole 
camera  as  in  Figure  2.9,  where  all  incoming  light  must  pass  through  the  optical  center  and 
is  projected  onto  a  focal  plane  located  at  distance  /  from  the  lens  [16].  The  focal  distance 
from  the  lens  to  the  focal  plane  is  crucial  to  the  calculations  of  ground  speed  in  the  same 
way  it  was  used  for  the  driftmeter  in  Section  1.1.2.  A  ground  feature  is  acquired  at  the 
very  edge  of  the  camera’s  FOV  and  its  centroid  is  represented  in  its  focal  plane  as  a  single 
pixel.  The  pixel  is  tracked  as  it  moves  in  the  camera’s  focal  plane  and  is  used  to  determine 
the  bearing  of  the  ground  feature. 


Figure  2.9:  Pinhole  camera  model.  The  centroid  of  the  ground  features  are 
tracked  as  a  pixel  representations  in  the  focal  plane. 


2.5.2  Bearing  Measurements. 

The  concept  of  bearing  measurement  tracking  used  as  an  automated  driftmeter  is 
presented  in  this  section.  This  concept  is  first  introduced  from  a  two-dimensional 
perspective  and  then  further  explained  for  three  dimensions.  Finally,  the  equations  used 
for  the  measurements  input  into  the  Kalman  filter  are  presented. 
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A  two  dimensional  representation  of  an  aircraft  flying  along  the  x  axis  while  tracking 
a  single  ground  feature  is  illustrated  in  Figure  2.10.  The  ground  feature’s  position  is 
represented  by  Xp  and  the  corresponding  pixel  location  in  the  focal  plane  is  represented  as 
Xf£,  where  ■£  indicates  a  discrete  time  instant.  The  aircraft  position  relates  to  its  horizontal 
distance  de  from  that  ground  feature  at  time  i.  As  the  ground  feature  is  tracked  across  the 
camera’s  FOV  in  the  focal  plane,  pixel  by  pixel,  the  camera  measures  the  ground  feature’s 
bearing  angle. 


Consider  the  simplified  two  dimensional  scenario  where  the  aircraft’s  distance  from 
the  tracked  ground  feature  is  determined  based  on  three  factors:  the  “known”  altitude  h, 
the  fact  that  pitch  angle  6  =  0,  and  the  measured  bearing  angle.  Thus,  the  camera  provides 
a  measurement  df  of  the  aircraft’s  horizontal  distance  directly  from  the  tracked  ground 
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feature  position  Xp  through  the  relationship  that  follows: 


de  _  Xfe 

'h~T 


(2.10) 


where  Xfc  is  the  location  of  the  ground  feature  image  in  the  focal  plane  of  the  camera. 

Transitioning  to  the  three  dimensional  scenario,  Figure  2.11  illustrates  that  the 
camera’s  focal  plane  is  placed  a  distance  /  in  front  of  the  camera.  This  is  done  in  order  to 
invert  the  image  in  the  focal  plane,  simplifying  the  calculations. 


Figure  2.1 1:  Three  dimensional  image  representation 


The  relationship  of  the  true  position  and  true  attitude  of  the  aircraft  to  that  of  a 
ground  feature  is  determined  by  the  attendant  geometry  and  is  given  by  the  Main  Equation 
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derived  in  [3]  that  follows: 


z 

where  Xf  and  yf  are  the  pixel  eoordinates  in  the  foeal  plane  of  the  projeeted  ground 
feature,  whose  true  position  is  Xp  and  jp,  and  /  is  the  eamera’s  foeal  length.  The  direetion 
eosine  matrix  previously  deseribed  in  (2.2)  transforms  a  veetor  eoordinatized  in  the 
body  frame  into  the  very  same  veetor  eoordinatized  in  the  navigation  frame.  The 
expression  ^xj,  +  +  /^  is  used  to  seale  the  line-of-sight  range  (Rlos)  between  the 

aireraft  and  the  ground  feature.  The  subseript  p  designates  the  position  in  the  navigation 
frame  of  the  ground  feature  and  the  eoordinates  (x,  y,  z)  without  subseripts  designate  the 
true  position  of  the  aireraft  in  the  navigation  frame. 

Reeall  that  the  navigation  frame  is  attaehed  to  the  flat  and  non-rotating  Earth. 
Equation  (2.11)  ean  then  be  separated  into  three  equations  as  follows: 


Xp 

Xf 

yp 

\RlOS  I 

yf 

^xj  +  yj  +  f 

-f 

Zp 

(2.11) 


Xp  -  X  = 


\Rlos  I 


xj  +  yj  +  p 


1  0  0 


yf 

-f 


yp-y 


\Rlos  I 


4 


x}  +  yf  +  r 


0  1  0 


Zp-Z 


\Rlos  I 


4 


x}+yf  +  f 


0  0  1 


Xf 

yf 

-f 

Xf 

yf 

-f 


(2.12) 


(2.13) 


(2.14) 
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Equation  (2.14)  can  be  rearranged  such  that  the  following  is  true: 


\Rlos  I 

^xj+yj  +  f^ 


Zn-Z 


0  0  1 


yf 

-f 


(2.15) 


It  is  assumed  that  the  elevation  Zp  of  the  tracked  ground  feature  is  known  to  be  zero.  Two 
measurement  equations  are  obtained  by  substituting  the  right  side  of  (2.15)  into  (2.12)  and 
(2.13),  which  yields  the  following  equation: 


Xp 

X 

Zp-Z 

_  yp  _ 

^f 

0  0  1 


yf 


1 

0 


0 

1 


0 

0 


yf 

-f 


This  is  further  simplified  by  multiplying  out  the  matrices  as  follows,  where  is  given  by 
(2.2): 


Xp 

X 

1 

Xf-yfip-f-e 

_  yp  _ 

Xfil/  +  yf  +  f-(f> 

Nondimensionalizing  such  that  the  following  is  the  case: 


yields  the  following  equation: 


Xp 

X 

Xf-yfip-e 

_  yp  _ 

—  \Zp  Z)  j  1 

-Xf  '  6  -\-yf  '  (p  -  \ 

yf  +  Xf-ij/f-cp 

This  result  can  now  be  separated  into  two  nonlinear  measurement  equations  as  follows: 


Xp  -  X  =  {Zp  -  z) 


yp-y  =  izp-  z) 


I  Xf-iff-yf-e  \ 

/  yf  +  if/-Xf  +  (f>  \ 

\-l-e- Xf  +  (f>-yfl 


(2.16) 

(2.17) 
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To  further  simplify  the  measurement  equations  we  ean  apply  the  binomial  approximation 
with  the  small  angle  assumption.  This  eauses  the  denominator  of  (2.16)  and  (2.17)  to  be 
multiplied  by  the  numerator  while  ehanging  the  signs  of  the  small  angles  as  follows: 

Xp-  x~  (Zp-  z)(xf  -  yf  •  i/r-  0)(-l  +  Xf  e  -yf  (2.18) 

yp-y~(Zp-  z){yf  +  Xf\f/  +  ^)(-l  +Xf6-yf(p)  (2.19) 

It  is  assumed  that  the  ground  feature  elevation  is  known  and  then,  without  loss  of 
generality,  Zp  is  set  equal  to  zero.  Equations  (2.18)  and  (2.19)  are  further  simplified,  sueh 
that  the  produets  of  the  small  angles  after  distribution  are  also  set  equal  to  zero,  thus 
yielding: 

Xp  -  X  X  z{xf  -  0(1  +  x^j)  +  Xf  y  f  (p  -  y  f  (2.20) 

yp-y~  z(yf  -  .r/  •  y/  •  0  +  0(1  +  yj)  +  Xfil/)  (2.21) 

The  states  and  measurements  are  then  perturbed  sueh  that  the  following  is  true: 


X  =  Xc  -  6x  y  = 

6  =  6c  -  66  0  = 

Xp  —  Xpc  6xpc  yp  — 

Xf  =  Xf,n  -  6xf  yf  = 


yc-6y 

t<\ 

II 

N 

1 

(pc  -  6(p 

II 

1 

ypc  ~  6y pc 

y  fm  ^y  f 

where  the  subseript  “e”  indieates  the  navigation  state  eomponents  provided  by  the  Free 
INS.  The  subseript  “m”  indieates  measured  quantities  of  the  pixels  in  the  foeal  frame. 
Inserting  the  perturbation  equations  into  the  measurement  equations  (2.20)  and  (2.21) 
yields  the  following: 

Xpc  6xpc  (Xc  0jc)  —  (zc  6z^^Xfjfi  6xf  (0c  d0)(l  "T  Xf^^  '^Xfjfi  •  6xf  6Xf) 

+  {Xfm  -  SxfXyfm  -  6yf){(f)c  -  6(f))  -  {y/m  -  6yf)(tf/c  -  6if/)] 
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Jpc  -  6ypc  -  {yc  -  Sy)  =  (Zc  -  Sz)\yfm  -6yf-{xfm-  5xf)(yfm  -  %)(0c  -  SO) 

+  {(pc  -  5(p){\  +  y]^  -  lyfm  ■  6yf  +  6y])  +  {xfm  -  6xf)(il/c  -  Stp)^ 
Due  to  the  small  error  in  the  measurements  and  the  small  angles,  the  products  of  these 
terms  can  be  neglected.  Therefore,  the  measurement  equations  can  be  further  reduced  to 
the  linear  forms  that  follow: 

Xpc  -  6Xpc  -  (Xc  -  dx)  =  (zc  -  Sz)(xf^  -dXf-iOc-  6e)(l  +  xjj 

{^fm  ‘  y fm){4^c  ^0)  y fm{^c  ^0)^ 


ypc  -  6ypc  -  CVc  -  6y)  =  {Zc  -  6z)\yfm  -6yf-{xf^-  yfndidc  -  S9) 

+  {(pc  -  5(p){\  +  y]j  +  Xf„i{ilJc  -  Sil/)^ 

By  moving  all  the  error  terms  (navigation  state  error  and  measurement  errors)  to  the  Right 
Hand  Side  (RHS)  and  all  the  non-error  terms  (INS-provided  navigation  state  and  pixel 
position  measurements)  to  the  Left  Hand  Side  (LHS)  of  the  two  measurement  equations, 
the  Kalman  filter  measurement  equations  are  obtained  as  follows: 

^pc  Zci^fm  9c{i  -t  ^ 0c  ■  ^fm  '  y fm  0c  "  y/mj  ~ 

^  ’  (2.22) 

-  5x  -  8z  -  Xf  -V  56{\  -I-  x^j)  -  6(p  ■  Xf  ■  yf  +  Sij/  ■  yf  +  6xpc  -  6xf 


ypc  yc  Zc{yfm  9c  ■  Xfm  •  yjm  "t  0c(l  "I"  y fm)  0c  ‘  ~ 

-  6y  -  6z-  yf  +  69  ■  Xf  ■  y/  -  6(p{l  +  y^)  -  6ifj  •  Xf  +  Sypc  -  Sy/ 


(2.23) 


2.6  Summary 

In  summary,  this  chapter  has  presented  the  mathematical  background  required  to 
understand  the  development  and  simulation  of  the  automated  driftmeter  fused  with  inertial 
measurements.  The  three  reference  frames  used  in  this  research,  strapdown  INS  concepts, 
Kalman  filtering  equations,  and  bearing  measurement  equations  were  introduced  in  this 
chapter.  Chapter  3  will  now  provide  the  specific  calculations  used  to  develop  the 
navigation  scenario  for  this  research. 
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III.  Methodology 


This  chapter  provides  a  detailed  description  of  the  simulations  conducted  and  the 
algorithms  used  in  this  research.  The  navigation  scenario  developed  in  [3]  and  followed 
up  in  [4]  is  first  presented.  Next,  the  continuous-time  INS  error  equations  developed  in 
this  research  from  the  merging  of  equations  (2.6)  and  (2.7)  is  introduced.  This  is  followed 
by  the  discrete-time  equivalent  INS  error  equations,  which  are  used  to  develop  the 
navigation-grade  INS  specifications  for  this  research.  After  these  I-cr  INS  error 
specifications  are  developed,  they  are  used  throughout  the  research  for  three  different  INS 
scenarios.  The  first  scenario  incorporates  the  error  specifications  for  the  accelerometers’ 
and  gyroscopes’  drift.  The  second  incorporates  the  error  specifications  for  the 
accelerometers’  and  gyroscopes’  biases.  Finally,  the  third  incorporates  a  combination  of 
these  two  error  sources.  This  presentation  method  is  employed  with  the  development  of 
geolocation  and  tracking  as  well  as  the  transitioning  between  these  tracked  ground 
features. 

3.1  Navigation  Scenario 

It  is  assumed  the  aircraft  is  flying  straight  and  level  at  a  constant  velocity  over  a  flat 
and  non-rotating  Earth.  All  of  the  system  variables  and  parameters  are 
non-dimensionalized  according  to  Table  3.1  [5].  With  the  aircraft  cruising  at  a  constant 


Table  3.1:  Non-dimensionalized  navigation  scenario  parameters 


Position 

Velocity 

Specific  Force  Error 

Angular  Rate  Error 

Time 

Vr 

Vx  ^  — 

A  y 

•P  V 

y^l 

Vy 

y  V 

6fy  f 

T  V 

T^Tl 

p? 

V,  ^  - 

4  V 

Z  V 
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nominal/true  altitude  h  =  1000  m,  the  nominal/true  velocity  is  v  =  100  The  gravity 
acceleration  is  assumed  to  be  g  =  10  ^  and  is  non-dimensionalized  according  to  the 
following: 


g 


h  .  g  1000  m  •  10  f  10000  ^ 


(100 


10000  K 


=  1 


(3.1) 


The  camera  frame  rate  is  the  same  as  the  sampling  frequency  fs  which  is  1  Hz.  The 
sampling  time  AT  is  non-dimensionalized  and  calculated  according  to  the  following: 


1  V  1  100  f 

fs  h  1  f  1000  m 


=  0.1 


(3.2) 


3.2  INS  Error  Equations 

It  is  necessary  to  develop  a  linear  dynamic  model  of  the  INS  errors.  The  flight 
scenario  is  highly  idealized  so  that  the  aircraft  dynamics  are  minimized  and  the  INS  errors 
are  modeled.  There  are  many  sources  of  errors  that  impact  the  performance  of  an  INS. 
Reference  [17]  states  that  significant  error  sources  include  alignment  errors,  sensor 
drift/bias  errors,  and  computational  errors.  In  this  research,  the  INS  alignment  errors  are 
assumed  to  be  zero.  An  evaluation  of  computational  errors  is  outside  the  scope  of  this 
research  and  will  not  be  addressed.  The  INS’s  accelerometer  and  gyroscope  sensors  each 
have  two  types  of  errors:  a  Gaussian  distributed  random  start-up  bias  error  and  a  Gaussian 
random  walk/drift  error. 

The  continuous-time  INS  navigation  state  error  dynamics  are  represented  in  state 
space  form  as  follows: 


d6x  =  (Ac  •  5x  -I-  Ec  •  6u)dt  +  T(.-dw 


(3.3) 


which  is  rewritten  as  follows: 


6x(t)  =  Ac  •  6x(t)  +  Ec  •  du(0  -I-  Ec  •  w(0 


(3.4) 


where  dx  is  the  navigation  state  error  vector  Ac  is  the  system  error  dynamics  matrix,  Ec  is 
an  input  matrix,  du  is  the  unknown  accelerometer  and  gyro  biases  vector,  and  w  is  a 
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Brownian  motion  vector  (which  quantifies  the  gyros’  and  accelerometers’  drift).  The 
navigation  state  error  vector  is  as  follows: 

=  [  6x,  6y,  6z,  Svx,  6vy,  6v^,  6^,  66,  6i//  (3.5) 

The  accelerometers’  and  gyros’  bias  errors  are  random  on  start-up  but  constant  thereafter. 
The  errors  are  expressed  as  follows: 


(b) 


Xb) 


Xb) 


(3.6) 


where  the  accelerometer’s  bias  errors  are  Gaussian  distributed  with  a  mean  of  zero 

and  a  standard  deviation  of  (Jb^.  The  gyroscope’s  bias  errors,  {6<jJ^^^)  are  also  Gaussian 


distributed  with  a  mean  of  zero  and  a  standard  deviation  of  cr^^.  They  are  described  as 
follows: 


6f^-N{0,  a-l) 


(3.7) 


6J^  -  N{0,  crl) 

The  vector  of  white  noise  processes  w  is  an  additive  zero  mean  Gaussian  process  with 
covariance  expressed  as  follows: 


£’{w(t)  •  w^(t  +  r)}  =  Q  •  6{t) 


where  6{t)  is  the  Dirac  delta  function.  It  accounts  for  high-bandwidth  error  sources  such 
as  electrical  noise  and/or  thermal  noise  in  the  gyros  and  accelerometers.  The  w  vector 
contains  the  random  walk  standard  deviation  scale  values  for  the  accelerometers  (cr^J  and 
gyroscopes  (cr^^),  is  expressed  as  follows 


w  = 


m  h  ■  <ri) 

N{0,  Ij  ■  ) 


«  -"exi 


(3.8) 


where  all  four  sensor  noise  standard  deviation  values  will  be  calibrated  for  this  navigation 
scenario  in  a  later  section. 
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The  error  dynamies  matrix  Ac  is  as  follows: 


Ac 


0  I3  0 
0  0 


0  0 


0 


'9x9 


(3.9) 


where  as  developed  in  (2.5),  is  the  skew-symmetrie  matrix  form  of  the  speeifie  foree 
veetor  in  the  navigation  frame. 

The  input  matrix  Fc  is  as  follows: 


Fc  = 


0 

c 


0 

0 


b 

0  c 


b 


(3.10) 


-■9x6 


where  the  matrix  is  the  body  to  navigation  frame  DCM  developed  in  (2.2).  At  eonstant 
altitude  flight  in  the  direetion  of  the  x  axis,  the  nominal/true  =  I3. 


3.3  Discrete-Time  Dynamics 

The  eontinuous-time  navigation  state  error  in  (3.4)  is  eonverted  to  diserete-time  with 
a  sampling  interval  of  AT,  ealeulated  in  (3.2).  The  diserete-time  navigation  error  state 
equation  is  then  as  follows: 


Sxk+i  =  Ad  •  5xk  +  Fd  •  6vik  -I-  Fc  • 


(3.11) 


where  the  subseript  k  represents  diserete  instanees  in  time. 

The  diserete-time  state  transition  matrix  Ad  is  ealeulated  from  the  eontinuous-time 
dynamies  matrix  Ac  aeeording  to  the  following: 


Ad  =  e 


.Ac-Ar 


Therefore,  we  ealeulate  matrix  Ad  as  follows: 

I3  I3  •  AT 

I 

Ad 


0 

0 


I3 

0 


_i  .  pw  ■  ^T^ 
_FW  .  AT 

I3 


-*9x9 
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which  for  the  substitutions  in  Section  3.1  quantifies  to  the  following: 

1  0  0  0.1  0  0  0  0.005  0 

0  1  0  0  0.1  0  -0.005  0  0 

0  0  1  0  0  0.1  0  0  0 

0  0  0  1  0  0  0  0.1  0 

Ad=  0  0  0  0  1  0  -0.1  0  0 

000001  0  00 

000000  1  00 

000000  0  1  0 

000000  0  0  1 

The  input  matrix  Fc  is  transformed  to  discrete-time  according  to  the  following 


I3-Ar  -i.F^-Ar^ 

0  h-AT  -i-F^-AT^ 

0  0  h-AT 
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This  integral  quantifies  as  follows: 


0.1  0  0  0.005 

0  0.1  0  0 

0  0  0.1  0 

0  0  0  0.1 


0  0  0  0 
0  0  0  0 
0  0  0  0 
0  0  0  0 


0 

0 

0 

0.000167 

0 

0.005 

0 

-0.000167 

0 

0 

0 

0.005 

0 

0 

0 

0 

0 

0 

0.005 

0 

0.1 

0 

-0.005 

0 

0 

0 

0.1 

0 

0 

0 

0 

0 

0.1 

0 

0 
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The  product  of  the  integral  and  Fc  yields  the  discrete-time  input  matrix  that  follows: 


^i3-Ar2 

h-AT 

0 


Fd  finally  quantifies  to  the  following: 

0.005  0  0 

0  0.005  0 


0 

0.1 

0 

0 

0 

0 

0 


0.1 

0 

0 

0 

0 


0 

0.1 

0 

0 

0 


•  •  AT^ 
■  •  AT^ 


h-AT 


0  0.005 

0  0 


-<9x6 


0 

-0.0001667 

0 

0 

-0.005 

0 

0.1 

0 

0 


0.0001667  0 

0  0 
0 
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0 


0 

0 
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0 

0 

0 

0 

0 

0 
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The  discrete-time  random  drift  error  vector  w^.  remains  the  same  as  the 


continuous-time,  random  drift  error  vector  in  (3.8)  as  follows: 

N(0.  h  ■  <rl) 

Wk  ~ 

N(o,  I, .  o-y 

where  it  remains  multiplied  by  the  continuous-time  input  matrix  Fc.  This  is  done  while 
taking  into  account  the  discrete-time  sampling  interval  AT.  Each  of  the  six  drift  errors  are 
Gaussian  distributed  and  randomly  selected  at  every  time  instant  k.  The  associated 
covariance  is  as  follows: 


(3.12) 

6x1 


E{yvk  •  w[}  =  Qd 


and  in  matrix  form  is  as  follows: 

Qd 


I3  •  cr] 


0  I3  •  < 


3.4  Augmenting  the  System  Dynamics  with  Constant  Biases 

The  navigation  state  error  vector  dx  is  augmented  with  the  bias  input  error  vector  du, 
because  the  bias  errors  are  random  on  system  start-up  but  constant  thereafter.  The 
augmented  navigation  state  error  vector  is  as  follows: 


dxao  -  NO,  0 


'9x9 

03x3 

03x3 

'3x3 

I3  •  crl 

‘-'a 

03x3 

'3x3 

03x3 

I3  •  cr; 

(3.13) 


Jl5xl5^ 


The  augmented  discrete-time  navigation  state  error  equation  is  as  follows: 

~  Ada  ■  ^ca  '  '^k  (- 

where  the  initial  covariance  accounts  for  the  uncertainty  in  the  original  nine  INS  error 
states’  alignment  and  the  Gaussian  distributed  random  bias  error  states.  The  state 
transition  matrix  Ad  is  augmented  with  the  discrete-time  input  matrix  Fd  as  follows: 

Ad  Fd 

Ada  =  (3 

06x9  le 


(3.14) 


(3.15) 
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The  continuous-time  input  matrix  Fc  must  be  augmented  with  zeros,  as  follows: 


r 


ca 


^6x6 


15x6 


(3.16) 


The  discrete-time  random  walk  vector  remains  the  same  as  specified  in  (3.12). 


3.5  Navigation  Grade  INS  Calibration 

For  this  simulation  the  bias  and  random  walk  measurement  errors  for  the 
accelerometers  and  gyroscopes  are  assumed  to  be  equally  responsible  for  the  INS  errors. 

A  covariance  analysis  of  the  INS  x  position  error  state  is  used  to  calculate  the  standard 
deviation  values  for  the  sensors  biases  and  drift,  which  will  cause  a  1  navigation  error. 
Using  the  discrete-time  Lyapunov  equation  that  follows: 

Pjt+l  =  Ada  •  Pit  •  Ada^  +  Fca  •  Qd  •  Tca^,  k  =  0, 1,  ...,3600  (3.17) 


The  state  covariance  matrix  P  (uncertainty  in  the  system  state  dynamics)  is  propagated 
throughout  a  one  hour  flight.  The  discrete  noise  covariance  matrix  Qd  (uncertainty  in  the 
system  noise  input)  is  constant  throughout  the  flight.  It  is  assumed  that  there  are  no  INS 
initial  alignment  errors.  Therefore,  the  navigation  state  errors  provided  in  (3.5)  at  the 
initial  time  step  (k  =  0)  are  all  zero  and  the  measurement  bias  errors  are  random  and 
Gaussian  distributed.  Measurement  bias  errors  are  represented  within  the  navigation  state 
error  initial  conditions  dxao  as  follows: 


dXao  = 


Ogxi 

^3x1 

73x1 


Jl5xl 


(3.18) 


where  ^  ~  N{0,  I3  •  cr^  )  and  7  ~  N{0,  I3  •  cr^  ).  The  uncertainty  at  start-up  is  specified  by 
the  accelerometers’  and  gyroscopes’  bias  standard  deviation  and  cr^^,  respectively. 
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Therefore,  the  navigation  state  error  covariance  matrix  P^.  is  initialized  as  follows: 


0 

Po  =  0 
0 

''S  ->15x15 

The  accelerometers’  and  gyros’  drift  is  described  as  follows: 


'9x9 

09x3 

09x3 

'3x9 

I3  •  crl 

03x3 

'3x3 

03x3 

I3  •  cr; 

(3.19) 


Qd  = 


I3  •  O-rf 
^3x3 


0 


3x3 


I3  •  crl 


(3.20) 


->6x6 


The  system  dynamics  are  propagated  at  the  non-dimensional  sampling  interval  AT  with 
the  Lyapunov  equation  shown  in  (3.17).  This  is  done  for  one  non-dimensional  hour,  3600 
time  steps,  in  order  to  determine  the  final  variance  value  of  ;c-position.  This  final  value 
constitutes  element  (1,1)  of  the  covariance  matrix  so  it  equals  (P36oo)i,i. 

This  propagation  is  repeated  four  times  to  determine  the  final  ;c-position  of  the 
variance  for  each  measurement  error.  Each  time  only  one  measurement  error  contributes  to 


the  1  ^  navigation  error.  This  is  represented  in  Table  3.2.  There  is  a  linear  relationship 


Table  3.2:  Final  x-position  variance  values  after  propagating  for  one  hour 
according  to  set  INS  sensor  error  parameters. 


Final  Value 

Accel  bias  cr 

Gyro  bias  cr 

Accel  drift  cr 

Gyro  drift  cr 

ab  =  (P360o)l,l 

=  1 

(Tb,=0 

II 

0 

II 

0 

l^b  =  (P3600)l,l 

0 

II 

CTb,  =  1 

(rb,=0 

II 

0 

ad  =  (P360o)l,l 

0 

II 

II 

0 

(Tb,  =  1 

II 

0 

l^d  =  (P360o)l,l 

0 

II 

b 

(Tb,=0 

II 

0 

(Td,  =  1 

between  the  uncertainty  in  the  accelerometer  and  the  gyro  variance,  which  can  be  directly 
related  to  the  aircraft’s  final  ;c-position  variance.  This  linear  relationship  is  as  follows: 

(P36oo)i,i  =  ab  •  crl^  +  pb  •  (tI^  +  ad  •  +  ySa  •  (3.21) 
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The  accelerometers  and  gyros  are  assumed  to  be  equally  at  fault  and  their  standard 
deviations  are  scaled  according  to  Table  3.3.  These  standard  deviation  values  are  now 


Table  3.3:  Calibrated  navigation-grade  INS  specifications  for  l|^. 


Final  x-Position  Variance 

Scaled  With 

Calibrated  INS  Sensor  Specifications 

ab  =  4.2037  x  10‘' 

1 

(Tb^  =  7.71 18  X  lO-i’ 

J3b  =  6.0567  X  1013 

1 

o-b^  =  6.4247  X  I0-* 

ad  =  1.5558  X  10* 

1 

ad,  =  4.0085  X  lO-^ 

fid  =  3.0254  X  1013 

1 

ad^  =  2.8746  x  lO'^ 

used  together  in  the  Lyapunov  equation  shown  in  (3.17)  to  propagate  the  .r-position 
variance  of  the  INS  throughout  the  one  hour  flight  and  verify  that  it  does  yield  the  1  ^ 
navigation  error.  Figure  3.1  illustrates  the  propagated  .r-position  variance  with 
accelerometer  and  gyroscope  noise  standard  deviations  according  to  the  calibrated  sensor 
specifications  in  Table  3.3. 
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Figure  3.1:  .r-position  error  standard  deviation  with  biases  and  process  noise 
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3.6  Free  INS 


The  drift  errors  for  the  simulated  Free  INS  are  developed  by  propagating  the 
discrete-time  stochastic  INS  error  equations  for  one  hour.  In  this  section  the  performance 
of  the  Free  INS  is  presented  with  just  drift  errors  sources,  just  bias  errors  sources,  and 
with  combined  error  sources.  The  calibrated  sensor  error  standard  deviation  values  in 
Table  3.3  are  used  throughout  this  research. 

3.6.1  Drift  Induced  Error. 

The  sensor  drift  errors  that  plague  an  INS  are  a  result  of  high-bandwidth  sources  such 
as  electrical  noise  and/or  thermal  noise  from  every  accelerometer  and  gyroscope  discrete 
measurement  throughout  the  flight.  The  result  of  the  noisy  measurements  is  a  random 
walk  superimposed  on  the  true  measurement.  The  random  walk  measurement  errors  will 
cause  the  errors  to  drift  either  positive  or  negative  of  the  true  measurement  with  a  strength 
drawn  from  a  zero  mean,  Gaussian  distribution.  The  strictly  drift-driven  navigation  state 
error  equation  is  extracted  from  the  original  form  of  the  discrete-time  navigation  error 
state  equation  shown  in  (3.11)  before  any  augmentation  as  follows: 


dx^+i  =  Ad  •  6xk  Fc  • 


(3.22) 


This  equation  is  propagated  with  k  throughout  the  one  hour  flight.  The  vector  of  white 
noise  processes  Wjt  is  randomly  selected  for  every  iteration  according  to  (3.12).  The 
covariance  of  the  drift-caused  navigation  state  error  is  calculated  using  the 
discrete-time  Lyapunov  equation  as  follows: 


P/t+i  -  Ad  •  P/t  •  Ad^  +  Fc  •  Qd  •  Fc^ 


(3.23) 


where  the  navigation  state  error  covariance  matrix  P  is  propagated  for  one  hour  to 
generate  a  68%  error  state  estimate  for  each  of  the  nine  state  values  throughout  the  flight. 
It  is  assumed  that  there  are  no  INS  alignment  errors.  Therefore,  the  navigation  state  errors 
provided  in  (3.5)  at  k  =  0  are  all  zero  and  there  is  100%  confidence  in  the  alignment.  This 
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confidence  is  inversely  proportional  to  the  uncertainty  in  the  alignment  (also  referred  to  as 
the  covariance  P)  The  covariance  matrix  for  the  Free  INS  alignment  is  therefore  as 
follows: 

f  0  0  1 


Po  = 


0 


0 


'9x9 


The  random  input  from  the  white  noise  processes  Wk  will  be  different  each  time  the 
simulation  is  run  and  consequently  so  will  be  the  navigation  state  error  dynamics. 
Appendix  B  contains  Figures  B.l,  B.2,  and  B.3,  which  illustrate  the  drift  error  and 
covariance  from  just  one  run  of  the  Free  INS  with  only  sensor  drift  error  inputs. 

3.6.2  Bias  Induced  Error. 

The  accelerometers’  and  gyroscopes’  bias  errors  are  random  on  start-up  but  constant 
thereafter.  Maybeck  [1]  refers  to  these  types  of  error  as  turnon- to-turnon  nonrepeatability 
biases,  because  they  change  from  one  period  of  operation  to  another.  They  should  not  be 
confused  with  alignment  errors  (said  to  be  zero  for  this  simulation)  that  occur  as  a  result 
of  the  error  between  the  aircraft’s  true  position,  velocity,  and  attitude  relative  to  where  the 
external  navigation  source  said  it  was  during  the  alignment.  After  alignment,  the  biases 
are  constant  and  cause  the  INS  measurements  to  be  offset  by  that  bias  until  the  next 
alignment.  Therefore  the  strictly  bias-caused  navigation  state  error  equation  is  only 
random  during  initial  alignment.  The  simulation  randomly  selects  the  bias  values  during 
initialization  from  a  Gaussian  distribution  described  in  (3.7).  They  then  remain  constant 
throughout  that  simulated  one  hour  flight. 

The  strictly  bias  state  space  error  equation  is  extracted  from  (3.14)  and  is  as 
follows: 


6'^ak+l  ~  Ada  •  5'S.ak 


(3.24) 


where  it  is  propagated  with  iterations  of  k  throughout  the  one  hour  flight  and  the  bias 
terms  are  augmented  into  the  state  transition  matrix  Ada.  The  covariance  of  the  navigation 
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state  error  caused  by  the  bias  is  calculated  using  the  augmented  discrete-time  Lyapunov 
equation  as  follows: 

Pjt+l  =  Ada  •  P/c  •  Ada^  (3.25) 

The  augmented  navigation  state  error  vector  dxa  is  initialized  according  to  (3.18). 
Therefore,  the  augmented  covariance  matrix  Pq  is  initialized  according  to  (3.19). 

The  covariance  for  the  Free  INS  will  be  driven  by  the  covariance  of  the  bias  terms 
and  will  be  the  same  every  time  the  simulation  is  run.  However,  the  random  choice  that 
creates  the  bias  terms  during  initialization  will  be  different  each  time  the  simulation  is  run 
and  the  propagated  error  will  be  different  each  time.  Appendix  B  contains  Figures 
B.4,  B.5,  B.6,  B.7,  and  B.8,  which  illustrate  the  bias-caused  error  and  covariance  from  just 
one  run  of  the  Free  INS  with  only  sensor  bias  error  inputs. 

3.6.3  Bias  and  Drift  Induced  Errors. 

The  combination  of  the  two  error  sources  creates  a  noisy  output  that  has  a  random 
walk  about  the  bias  relative  to  the  true  measurement.  If  the  true  measurement  were  zero, 
then  the  bias  offset  would  be  added  first  and  then  the  random  walk  would  be  added  to 
cause  a  drift  around  the  bias.  The  navigation  state  error  equation  with  both  error  sources  is 
the  same  as  presented  in  (3.14).  It  is  presented  again  here  for  reference: 

^^a.k+\  ~  A(ja  •  -|-  Fga  • 

The  navigation  state  error  is  propagated  for  one  hour  and  its  covariance  Pjt+i  is  calculated 
using  the  following  equation: 

Pjt+l  =  Ada  •  Pjt  •  Ada^  +  Tea  •  Qd  '  Pja^  (3.26) 

The  realization  of  the  drift-caused  error  and  the  bias  error  will  be  different  every  time  the 
simulation  is  run.  Appendix  B  contains  Figures  B.9,  B.IO,  B.ll,  B.12,  and  B.13,  which 
illustrate  the  error  and  covariance  from  just  one  run  of  the  Free  INS  with  drift  and  bias 
sensor  error. 
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3.6.4  Free  INS  Output. 

The  errors  developed  in  this  section  from  propagating  the  Free  INS  must  be  added  to 
the  true  state  of  the  simulated  aircraft  flight  in  order  to  establish  an  INS  output  of  a 
moving  aircraft.  As  mentioned  in  Section  3.1,  the  aircraft  is  flying  straight  and  level  at  a 
constant  velocity  over  a  flat  and  non-rotating  Earth.  Therefore,  the  only  true  dynamics  of 
the  simulated  aircraft  is  the  movement  in  the  x  direction.  This  movement  is  due  to  the 
aircraft’s  constant  velocity,  which  directly  relates  to  the  true  aircraft  position.  Therefore, 
the  output  of  the  Free  INS  is  calculated  according  to  Table  3.4,  where  the  subscript  c 
indicates  that  the  value  is  the  INS -calculated  output.  These  calculated  INS  output  values 
are  used  in  the  measurement  equation  to  establish  the  measurements  provided  to  the 
Kalman  filter. 


Table  3.4:  Free  INS  Output 


Position 

Velocity 

Attitude 

Xc  =  t  +  6x 

=  1+  6v^ 

II 

II 

4= 

II 

0,  =  69 

Zc  =  i  +  6x 

II 

i]/c  =  6i]j 

3.7  SLAM 

A  measurement  epoch  n  (period  of  time)  is  initiated  by  the  geolocation  of  a  new 
ground  feature.  Without  loss  of  generality  it  is  simulated  that  the  ground  features  are 
arranged  such  that  they  are  evenly  spaced  1000  m  apart.  With  the  aircraft  moving  at  a 
constant  speed  of  100  ^  ground  speed  in  the  direction  of  the  ground  features,  each  epoch 
is  IO5  long.  Therefore,  the  simulation  of  a  one  hour  flight  will  have  360  epochs,  and 
n=  ...,  N,  where  N  =  360.  Camera  measurements  are  taken  at  the  same  interval  that  the 
Free  INS  is  sampled  AT  =  1  i’,  so  there  are  10  camera  measurements  per  measurement 
epoch.  According  to  [19],  at  least  two  ground  features  must  be  tracked  at  all  times  in  order 


50 


to  maintain  observability  when  bearing-only  measurements  are  used.  Observability  is 
increased  as  more  ground  features  are  tracked.  However,  this  work  assumes  only  two 
ground  features  are  tracked  in  each  measurement  epoch. 

The  first  epoch  {n  =  1)  is  different  from  the  remaining  epochs  because  in  the  first 
epoch  the  camera  simultaneously  geolocates  two  ground  features.  The  first  ground 
feature,  Xp^,  is  at  1000  m  and  the  second,  Xp^,  is  at  2000  m.  These  factors  are 
non-dimensionalized  according  to  Table  3.1  to  =  1  and  Xp^  =  2.  A  two-dimensional 
representation  of  the  geolocated  ground  features  for  the  first  epoch  is  illustrated  in 
Figure  3.2.  The  Free  INS  is  perfectly  aligned.  The  INS  .r  calculated  position,  Xc,  is  exactly 
zero.  The  INS  z  calculated  position,  Zc,  is  exactly  one.  The  INS  y  calculated  position,  jc,  is 
exactly  zero,  and  is  not  illustrated. 

The  geolocated  ground  features  are  acquired  while  the  aircraft  position  is  exactly 
known  with  noisy  camera  measurements.  Therefore,  the  first  two  geolocated  ground 
feature’s  positions  are  calculated  according  to  Table  3.5,  where  the  geolocation  state  error 
values  6xpc  and  6ypc  are  on  average  zero,  with  an  uncertainty  associated  with  the  standard 
deviation  of  the  camera  measurement  cr^. 

Table  3.5:  Calculated  non-dimensional  position  measurements  for  first  two 
ground  features 


Ground  Feature  #  1 

Ground  Feature  #  2 

Xpcy  -  1  H"  5Xpc^ 

Xpc2  —  2  "h  SXpc2 

Jpc^  =  0  +  6ypc, 

ypc2  =  0  +  3ypc2 
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'^pCi 


Figure  3.2:  First  Epoch  where  the  first  two  ground  features  are  geolocated 


The  navigation  state  error  vector  6x  is  augmented  with  the  geolocation  error  vector  ds 
as  follows: 

r 

SXn 


6s  = 


'■pc  I 

Sypci 

^^pC2 

5ypc2 


(3.27) 


J4xl 


which  accounts  for  the  accumulated  error  from  acquiring  a  ground  feature  while  using  a 
noise-corrupted  bearing  measurement  and  the  error  prone  INS-provided  ownship  position. 
For  the  first  epoch  it  is  assumed  that  the  aircraft  position  measurements  are  known  exactly. 
However,  the  camera  measurements  are  noise-corrupted  with  a  standard  deviation  cTc. 
Therefore,  the  initial  geolocation  error  vector  dso  is  as  follows: 


dSn  — 


^^pC{ 

pc  \ 

Ay 

^•^pC2 

6ypc2 

4x1 

(3.28) 


J4xl 


where  the  geolocation  error  is  on  average  zero  and  is  included  in  the  augmented 
navigation  state  error  vector  dx,  while  the  associated  variance  is  captured  in  the 
augmented  covariance  matrix  P.  The  following  subsections  present  how  the  augmentation 


52 


for  SLAM  is  accomplished  for  the  three  different  methods:  strictly  INS  drift/process  noise 
error,  strictly  INS  bias  error,  and  combined  INS  drift  and  bias  errors. 

3.7.1  SLAM  with  INS  Process  Noise  Only. 

The  errors  associated  with  geolocating  two  ground  features  are  augmented  into  the 
navigation  state  error.  The  augmented  dynamics  are  as  follows: 


=  Ads  •  dxs,  +  Tcs  •  w, 


(3.29) 


where 


dxc 


6x 

6s 


(3.30) 


-■13x1 


The  state  transition  matrix  Ads  and  the  noise  input  matrix  Fcg  account  for  the  geolocation 
error  and  are  augmented  as  follows: 


^ds 


Ad  09x4 
04x9  I4 


-■13x13 


r  = 

^  CS 


Tc 

O4X6 


(3.31) 


(3.32) 


-■13x6 


The  discrete-time  random  drift  vector  is  the  same  as  presented  before  in  (3.12).  Since 
the  INS  initial  alignment  is  assumed  perfect,  and  the  geolocation  errors  are  on  average 
zero,  the  augmented  navigation  state  error  vector  is  initialized  with  a  vector  of  zeros  as 
follows: 


6x, 


■So 


0 


0 


(3.33) 


-■13x1 


and  the  associated  covariance  is  initialized  according  to  the  following  equation: 


0 


09x9 

^4x9  I4  ^  c 


'9x4 


04x9  I4  •  CT? 


-■13x13 
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3.7.2  SLAM  with  INS  Bias  Errors  Only. 

The  augmented  navigation  state  error  vector  in  (3.24)  includes  the  INS  bias  errors.  In 
this  section,  the  state  vector  is  augmented  again  to  include  the  geolocation  error  vector  ds 
as  follows: 


Adas  ■  dXg 


(3.34) 


The  new  augmented  state  transition  matrix  Adas  is  as  follows: 


Adas  — 


Ad  Td  09x4 

06x9  l6  06x4 
04x9  04x6  I4 


(3.35) 


L  -'19x19 

This  augmented  navigation  state  error  vector  is  initialized  with  a  vector  of  zeros  because 
the  error  from  the  first  nine  states  is  assumed  to  be  zero,  the  error  from  the  six  INS  bias 
states  is  assumed  to  be  on  average  zero,  and  the  four  states  that  track  the  geolocation  error 
for  the  first  two  ground  features  is  on  average  zero.  This  is  expressed  as  follows: 


^*aso  ~ 


0 


0 


(3.36) 
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The  associated  covariance  is  initialized  according  to  the  following  equation: 
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'9x9 

^9x3 

^9x3 

09x4 

'3x9 

I3  •  (^l 

03x3 

03x4 

'3x9 

^3x3 

I3  •  CTfo 
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^3x3 

03x3 

I4  •  cr; 

J  19x19 


where  the  first  nine  entries  represent  the  navigation  state  error  and  the  uncertainty  is  zero 
because  of  perfect  INS  alignment.  The  next  six  entries  represent  the  standard  deviation  of 
the  accelerometers’  and  gyroscopes’  biases.  The  last  four  entries  represent  the  uncertainty 
in  the  camera  measurements.  This  method  of  augmenting  for  geolocation  error  is  applied 
again  for  the  combined  INS  error  scenario  in  Section  3.7.3. 
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3. 7.3  SLAM  with  INS  Drift  and  Bias  Errors. 

The  augmented  navigation  state  error  equation  in  (3.14)  includes  the  INS  bias  errors 
and  the  INS  process  noise  errors.  In  this  section,  the  geolocation  state  error  ds  is  also 
augmented  into  the  dynamics  as  follows: 


Teas  Wyt 


(3.37) 


where  the  process  noise  terms  are  simply  added  to  the  navigation  state  error  equation  from 
the  previous  section.  The  process  noise  input  matrix  Teas  is  augmented  to  account  for  the 
geolocation  error  as  follows: 


F  - 

A  ras  — 


(3.38) 


where  the  zeros  in  (3.38)  do  not  allow  the  random  drift  error  vector  to  influence  the 
geolocation  state  error  terms.  The  navigation  state  error  vector  and  the  associated 
covariance  matrix  are  initialized  as  in  Section  3.7.2. 


3.8  The  Kalman  Filter 

The  Kalman  filter  algorithm  optimally  combines  noise-corrupted  bearing 
measurements  from  the  camera  with  noise-corrupted  accelerometer  and  gyroscope 
measurements,  to  determine  an  estimate  of  the  INS  navigation  state  error.  The  algorithm 
uses  the  Free  INS  navigation  state  error  dynamics  to  propagate  the  navigation  state  error 
estimate  dx  for  the  next  discrete  time  instant  according  to  the  system  model.  The 
algorithm  then  updates  the  estimate  with  the  knowledge  of  the  camera  measurement  z  to 
determine  an  optimal  estimate  dx  of  the  INS  navigation  state  error.  This  optimal  estimate 
is  then  subtracted  from  the  Free  INS  calculated  navigation  state  Xc  to  determine  an 
improved  navigation  state  estimated.  ^ is  expressed  as  follows: 

^  =  X  -I-  dx  -  dx^  (3.39) 

The  algorithm  is  illustrated  in  block  diagram  form  in  Figure  3.3. 
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Figure  3.3:  INS  Aiding  Using  a  Kalman  Filter 


During  a  measurement  epoch,  the  Free  INS  error  equations  described  in  (3.29), 
(3.34),  or  (3.37)  (according  to  the  system  being  modeled)  are  updated  using  the 
measurement  equation  that  follows: 

Zf  =  +  ye  (3.40) 


where  the  subscript  i  =  \, ...,  L  represents  discrete  instants  of  time  within  each  epoch  and 
L  is  the  last  measurement  within  that  epoch.  The  optical  measurement  at  time  instant  £ 
is  related  to  the  navigation  state  error  dx  through  the  observation  matrix  H  plus  the 
measurement  error  v.  The  measurement  error  is  a  white  Gaussian  sequence  with  a 


covariance  R  expressed  as  follows: 

R  = 


cr^  0  0  0 

0  crl  0  0 

0  0  crl  0 

0  0  0 


(3.41) 


Calculation  of  ctc  is  based  on  the  measurements  in  the  camera’s  focal  plane  having  a  one 


pixel  error.  Therefore,  the  camera’s  aspect  ratio  of  one  and  its  resolution  (MP  count) 
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determine  the  standard  deviation  cr,.  of  the  camera,  calculated  as  follows: 


(Tc  =  a/ •  10-6  (3.42 

V  #  MP  ^ 

where  #MP  is  the  resolution  of  the  camera. 

The  4x1  measurement  vector  contains  the  measurements  of  the  geolocated 
ground  features  in  the  two  dimensional  x  and  y  plane,  relative  to  the  position  of  the 
aircraft.  The  first  and  second  measurement  terms  relate  to  the  first  ground  feature’s  {x,  y) 
position  and  the  third  and  fourth  measurement  terms  relate  to  the  second  ground  feature’s 
(a,  y)  position.  These  measurements  are  best  explained  by  relating  the  measurement  dc  in 
the  two  dimensional  illustration  in  Figure  2.10,  where  the  bearing  measurement  provides 
the  position  of  the  aircraft  relative  to  the  position  of  the  ground  feature. 

The  measurement  vector  is  derived  from  the  geolocated  ground  feature 
measurements  at  the  beginning  of  each  epoch,  the  Free  INS  measurements  from  the 
beginning  of  the  navigation,  and  the  optical  measurements  in  the  focal  plane  of  the 
geolocated  ground  features  throughout  each  epoch.  Measurement  vector  z^  is  developed 
from  the  LHS  of  (2.22)  and  (2.23)  as  follows: 


Xpci  ~  Xci^  —  ZcJyXfmlf  —  0ct(l  +  ^fmle^  ‘  Xfml(  '  y fm\t  ~  4^Ck  '  y fm\^ 

ypci  ~  yck  ~  ZcJ^fmli  ~  dci;  ■  Xfmli  '  y fmlt  +  0cj(l  +  y fmlc^  ‘  -^/mlc) 

Xpc2  ~  ~  Zci,i^Xfm2e  ~  ^ct(l  +  ^fm2^  ‘  Xfmlc  '  y fm2t  ~  4^Ck  '  y fm2^ 

ypc2  ~  yck  ~  Zck{yfm2(  ~  dck  ■  Xfm2t  '  y fm2c  +  ^cj(l  +  y '  Xfm2^ 


'4x1 


(3.43) 


where  the  geolocated  ground  feature  measurements,  {Xpc^,ypc^,  Xpc2,  and  remain 
constant  throughout  each  measurement  epoch  n,  because  the  tracked  ground  features  are 
stationary.  These  measurement  values  are  presented  in  Table  3.5  for  the  first  epoch  where 
the  position  of  the  aircraft  is  initially  known  exactly  and  the  only  error  in  geolocation  is 
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from  the  camera  measurement.  However,  the  geolocated  ground  feature  measurements  for 
the  remaining  epochs  are  established  from  the  aided  (corrected)  INS-provided  aircraft 
position  and  the  camera  measurement  of  the  ground  features.  The  INS-calculated 
navigation  state  values  are  drawn  directly  from  the  Free  INS  according  to  Table  3.4  for  the 
one  hour  flight.  The  camera  measurements  in  the  focal  plane  (.v/mn,  J/mn,  Xfmin  and 
include  the  error  from  the  camera  and  are  taken  throughout  each  epoch  at  each  discrete 
time  instant  These  camera  measurements  are  iterated  according  to  £  and  are  calculated 
as  in  Table  3.6,  where  ^  is  the  error  from  the  camera  measurement  drawn  randomly 
according  to  the  Gaussian  distribution,  N(0,  cr^).  The  calculations  embodied  in  (3.40)  and 
(3.43)  are  represented  in  the  block  labeled  ’’Measurement  Generator”  in  Figure  3.3. 


Table  3.6:  Pixel  measurements  in  focal  plane  over  time  for  the  and  2”^ 
ground  features 


time 

Xf,„  for 

yf,n  for 

Xf,n  for  2”^ 

for  2”"^ 

II 

o 

=  1-0  +  C 

yfmig  =  0  +  ^ 

Xfm2g  ~  ^'0  +  4" 

y/mzo  =0  +  4: 

£=  1 

—  0.9  +  ^ 

y/mi|  =  0  +  ^ 

Xfm2^  —  1-9  +  4' 

yfm2^  =  0  +  4” 

II 

Xfmig  =  0- 1  +  ^ 

yfmig  =0  +  4' 

~  1  •  1  +  C 

y/m2,  =  0  +  4: 

II 

o 

~  0.0  +  4" 

yf ’"ho  ~ 

~  1-0  +  4' 

y/miiQ  =0  +  4' 

The  observation  matrix  is  a  4  x  9  matrix,  which  relates  the  optical  measurements 
in  the  focal  frame  to  the  navigation  state  error  vector  shown  in  (3.5)  in  the  navigation 
frame.  It  is  augmented  according  to  the  specific  navigation  state  error  vector  configuration 
being  considered.  The  observation  matrix  is  generated  using  the  RHS  of  (2.22)  and  (2.23) 
for  both  ground  features’  bearing  measurements.  The  first  and  second  rows  relate  to  the 
first  ground  feature’s  x  and  y  coordinates  and  the  third  and  fourth  rows  relate  to  the  second 
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ground  feature’s  x  and  y  coordinates.  is  as  follows: 


H,  = 


-1 

0 

0 

0 

0 

-Xfh  ■  y/if 

1  + 

y/h 

0 

-1 

-yfie 

0 

0 

0 

-i^+yjo 

Xfif 
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-1 

0 
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0 

0 

0 

-Xf2t  •  y/2, 

1  + 

■^/2; 

yne 

0 

-1 

-yf2t 

0 

0 

0 

-(l+yj2,) 

'yf2( 

-^f2e 

(3.44) 


-<4x9 

Since  the  aircraft  is  moving  in  the  positive  x  direction,  and  in  the  first  measurement  epoch 
the  ground  features  are  located  at  1000  and  2000  meters  in  the  positive  x  direction,  the 
Xfi^  and  Xf2f  non-dimensional  measurements  in  the  camera’s  focal  plane  are  initially 
recorded  as  one  and  two.  These  recorded  measurements  decrease  relative  to  the  aircraft’s 
constant  forward  velocity.  Therefore,  the  tracked  ground  features’  x-direction  pixel 
location  values  decrease,  while  y-direction  pixel  location  values  remain  at  zero  according 
to  Table  3.7  throughout  each  epoch. 


Table  3.7:  Pixel  values  in  observation  matrix  over  time  for  the  and  2”^ 
ground  features 
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3.8.1  Kalman  Filter  with  only  INS  Drift  Error. 

This  section  presents  the  equations  used  to  propagate  and  update  the  navigation  state 
error  estimate  of  the  Free  INS  with  only  process  noise  errors.  The  recursive  KF  algorithm 
begins  by  initializing  a  loop  in  each  measurement  epoch  when  i  =  0  and  when  the  two 
ground  features  are  geolocated.  Since  it  is  assumed  that  the  INS  alignment  is  perfect  in  the 
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first  measurement  epoch  {n  =  1),  the  KF  is  initialized  using  the  following  equation: 


•  - — -+\«=i 

j 


0 


0 


13x1 


(3.45) 


where  the  superscript  plus  sign  indicates  that  it  is  an  optimal  estimate  of  the  navigation 
state  error.  The  four  geolocation  error  estimates  are  on  average  zero.  The  product  of  the 
optimal  state  estimate  and  the  state  transition  matrix  Ajs  provides  a  propagated  navigation 
state  error  estimate  dx  for  the  next  discrete-time  instant,  as  follows: 


—  Ads  ■  dXs^ 


(3.46) 


where  the  superscript  minus  sign  indicates  that  it  is  before  the  camera  measurements  have 
been  received. 

The  uncertainty  in  the  KF  navigation  state  error  estimates  are  captured  in  the 
covariance  matrix  for  each  discrete  time  iteration  £.  The  initial  covariance  matrix  Pq 
describes  the  confidence  in  the  initial  alignment  and  describes  the  navigation  state  error 
estimate  in  (3.45)  where  the  superscript  plus  sign  indicates  that  it  is  the  covariance  of  the 
optimal  navigation  state  error  estimate. 

In  order  to  maintain  consistency  with  [4],  the  uncertainty  in  position  at  the  beginning 
of  the  navigation  scenario  is  assumed  to  be  one  meter,  while  the  uncertainty  in  velocity  is 
assumed  to  be  10“^  z  mm/s,  and  the  uncertainty  in  the  aircraft  attitude  is  assumed  to  be  20 
arc  seconds.  Thus,  the  non-dimensional  navigation  state  error  covariance  values  are  as 


dPx, 

dPy, 

dPz  - 

'  A(03x1, 

1  X  10“ 

dVx, 

dVy, 

dVz- 

^  A(03x1, 

1  X  10" 

d^^ 

-A^(03xU 

,  1  X  10 
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follows: 


The  initial  covariance  matrix  for  the  KF  also  includes  the  covariance  of  the  geolocation 
error  terms  described  in  (3.28)  as  A^(0,  cr^),  which  is  unique  for  the  initialization  of  the 
first  epoch.  The  initialization  of  the  following  epochs  is  discussed  in  Section  3.9.  The  KF 
initial  state  covariance  matrix  Pq  is  as  follows: 


P 


+ 

0 


I3  •  10-^  000 

0  l3- 10-^6  0  0 

0  0  I3  •  10-^  0 

0  0  0 


l4-cr2 


Jl3xl3 


(3.47) 


The  propagated  KF  covariance  of  the  navigation  state  error  estimate  at  time  instant  (^  +  1) 
is  calculated  by  solving  the  Lyapunov  equation,  as  follows: 


P^+l  =  Ads  •  Pf  •  Ads^  +  Pcs  •  Qd  •  Pcs^ 


(3.48) 


where  the  superscript  minus  sign  indicates  that  it  is  the  covariance  of  the  suboptimal 
navigation  state  error  estimate  and  is  calculated  according  to  the  following: 

£j(dx-dx^^i)(5x-5x^^i))  (3.49) 

The  Kalman  gain  K^+i  is  calculated  as  follows: 

Kf+i  =  P^^i  •  HsJ+i(Hs^+i  •  P^^i  •  HsJ^i  +  R)  ^  (3.50) 


where  the  observation  matrix  H  in  (3.44)  is  augmented  to  account  for  the  geolocation 
error  terms  as  follows: 
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sC  - 


H 


4x9 


I4 


I4xl3 


(3.51) 


Through  this  augmentation,  the  geolocation  error  terms  in  (3.28)  directly  enter  the 
measurement  equation  (z  =  H  •  dXg  +  v).  The  Kalman  gain  is  then  used  to  establish  the 
navigation  state  error  estimate  dxs^^j  for  the  discrete  time  instant  i  +  \,  after  the 
measurement  zt+\  has  been  received. 


61 


An  update  to  the  navigation  state  error  estimate,  occurs  when  the  measurement 
z  from  the  optical  sensor  has  been  received.  The  optimal  navigation  state  error  estimate 
is  determined  according  to  the  following  equation: 

dxj^i  =  dxs^^i  +  -  Hs^+i  •  dxs^+i)  (3.52) 


The  covariance  of  the  estimation  error  for  the  navigation  state  error  at  time  ^  +  1  is 
calculated  as  follows: 

P^+i  =  (li3  -  K/+1  •  Hs^+1  (3.53) 


The  recursive  loop  starts  over  by  propagating  with  (3.46)  and  (3.48).  This  loop  continues 
until  time  £  =  10  =  L.  After  L  =  10,  the  first  ground  feature  is  no  longer  in  the  camera’s 
FOV  and  a  new  ground  feature  must  be  geolocated. 

3.8.2  Kalman  Filter  with  only  INS  Bias  Error. 

This  section  presents  the  equations  used  to  propagate  and  update  the  navigation  state 

error  estimate  for  the  Free  INS  model  with  only  bias  noise  errors.  The  algorithm  is  the 

same  as  presented  in  Section  3.8.1,  however  the  augmentation  of  the  bias  terms  into  the 

state  transition  matrix  must  be  handled  differently.  As  with  the  previous  algorithm,  the 

recursive  loop  begins  when  the  ground  features  are  geolocated  and  the  INS  is  aligned  with 

the  external  navigation  source  and  thus  provides  an  optimal  navigation  state  error  estimate 
- 1- 

dx  as  follows: 

[ol 


- +\n=l 

^*aso  j  ~ 


(3.54) 


0 


19x1 


where  the  nine  navigation  state  error  estimates  are  aligned  with  the  external  navigation 
source  and  said  to  be  zero,  the  six  bias  error  estimates  are  on  average  zero,  and  the  four 
geolocation  error  estimates  are  on  average  zero.  The  navigation  state  error  estimates  are 
propagated  with  the  state  transition  matrix  which  accounts  for  the  bias  terms  according  to 
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the  following  equation: 


6x 


ast+\ 


(3.55) 


The  initial  covariance  matrix  Pq  for  the  bias  error  model  will  be  the  same  as  with  the  drift 
error  model  in  (3.47),  except  it  will  include  the  covariance  for  the  bias  terms  as  shown  in 
the  following  equation: 


P 


+ 

0 


I3  •  10-6  0  0 
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(3.56) 


It  is  propagated  for  time  instant  i  +  \  with  the  state  transition  matrix  according  to  the 
following  equation: 

=  Adas  •  p;  •  Adas^  (3.57) 


The  Kalman  gain  K^+i  is  calculated  the  same  as  in  Section  3.8.1,  as  shown  in  the 
following  equation: 
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■  Has^^l(Hasf+l 
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e+i 


•  HasJ+i  +  R) 


(3.58) 


however,  the  observation  matrix  H  in  (3.44)  is  augmented  to  exclude  the  bias  error  terms 
and  include  geolocation  error  terms  as  follows: 
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ast 
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(3.59) 


where  the  bias  error  terms  in  the  navigation  state  error  vector  dXas^  do  not  impact  the  error 
from  the  camera  measurement  z.  The  geolocation  error  terms  on  the  other  hand  directly 
influence  z. 
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The  Kalman  gain  is  then  used  to  establish  the  navigation  state  error  estimate  dxas^+i 
for  the  next  diserete  instant  after  the  measurement  z  has  been  reeeived. 

An  update  to  the  navigation  state  error  estimate  dxas^+i  oeeurs  when  measurement  z 
from  the  optieal  sensor  has  been  reeeived.  The  optimal  estimate  dxas^+i  is  determined 
using  the  following  equation: 

dXas^+l  =  dXas^^.1  +  Kf+i^Zf+1  —  Has^+l  ‘  dXas^+1  j  (3.60) 

The  eovarianee  for  the  optimal  estimate  at  time  ^  +  1  is  ealeulated  as  follows: 

P^+l  =  (ll9  -  •  Has^+i)p^+l  (3.61) 

The  reeursive  loop  starts  over  by  propagating  with  (3.55)  and  (3.57).  This  loop  eontinues 
until  the  first  ground  feature  is  no  longer  in  the  eamera’s  FOV  and  a  new  ground  feature  is 
geoloeated  at  time  L. 

3.8.3  Kalman  Filter  with  Both  INS  Error  Sources. 

This  seetion  presents  the  equations  used  to  propagate  and  update  the  navigation  state 
error  estimate  for  the  Free  INS  model  with  both  sensor  drift  errors  and  sensor  bias  errors. 
This  is  simply  a  eombination  of  Seetions  3.8.1  and  3.8.2  .  The  initial  eonditions  are  drawn 
from  (3.54)  and  (3.56).  The  reeursive  equations  are  presented  in  Figure  3.4. 

3.9  Transitioning  from  epoch  n  to  epoch  n  +  \ 

At  the  beginning  of  navigation  the  aireraft  navigation  state  is  perfeetly  known  and 
eonsequently  the  navigation  state  error  assoeiated  with  a  perfeet  alignment  is  zero.  At  the 
very  instant  the  alignment  is  established,  the  aireraft  eamera  geoloeates  two  ground 
features.  Their  measurements(.rpci,  Jpc,,  Xpc^,  and  jpc^)  are  established  in  (3.28)  and  remain 
eonstant  throughout  the  first  measurement  epoeh.  They  are  used  in  the  KF  measurement 
veetor  z  in  (3.40)  to  establish  bearing  measurements  whieh  are  used  to  ultimately  aid  the 
INS  eourtesy  of  the  KF  algorithm. 
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Figure  3.4:  Linear  Kalman  Filter  Recursive  Loop 

At  the  completion  of  the  first  epoch,  the  first  ground  feature  (whose  coordinates  are 
Xpc^  and  ypc^)  is  no  longer  in  the  camera’s  FOV  and  the  coordinate  measurements  are 
discarded.  Simultaneously,  Xpc^  and  jp^^  are  transitioned  to  be  tracked  as  the  new  first 
ground  feature,  whose  coordinates  are  Xpc^  and  in  the  new  measurement  epoch  n  +  1 . 
This  new  measurement  epoch  is  marked  by  the  geolocation  of  a  new  ground  feature  that 
replaces  Xpc-^  and  of  tho  previous  epoch  n.  This  is  illustrated  in  Figure  3.5  where  the  y 
coordinate  is  not  shown. 

The  aircraft’s  corrected  navigation  state  x"  is  used  to  establish  the  location  of  the  new 
ground  feature  and  thus  initializes  the  next  epoch  n  +  1  at  time  £  =  0.  The  ground  feature 
measurements  for  this  new  epoch  are  derived  from  (2.16)  and  (2.17)  utilizing  the 
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Figure  3.5:  Transition  from  Epoch  n  to  Epoch  n  +  I 


improved  navigation  state  estimate  x  as  follows: 
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=  v” 

J'pci  / 


pC2 


^pcl  =  -  Sx)  +  [zc,  -  5z) 

y7cl  =  [y^L  -  sy)  +  -  &,) 


■^fm2o  ~  (<Acl  ~  ^4^ ^yfmlo  ~  (dci  ~  j 
^  (^cl  ~  ~  {‘Pcl  ~  ^4^i^yfm2o 

yfm2o  +  (<Aci  ~  m2o  +  (0CZ.  - 


(3.62) 


1  {^cl  ^^i^^fm2o  {^ct  ^4^ i^y fm2o 

where  the  terms  with  a  hat  are  KE  estimates  of  the  specific  navigation  state  error 
components.  These  estimates  are  subtracted  from  their  corresponding  Eree  INS  outputs, 
indicated  with  a  subscript  cl  where  L  indicates  the  last  measurement  of  the  previous 
epoch.  This  provides  the  corrected  navigation  state,  which  is  used  with  the  Xf  and 
camera  measurements  in  the  focal  plane  during  geolocation  of  the  new  ground  feature. 
These  focal  plane  measurement  values  for  the  second  ground  feature  at  instant  ^  =  0  are 
presented  in  Table  3.6  and  again  as  follows  for  convenience: 


=  2.0  +  4- 


y  /'”2o  =  0  +  ^ 
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where  ^  ~  N(0,  (t\).  The  geolocated  measurements  Xpc^^ypc^,  Xpc^,  and  jpc^  are  used  in  the 
calculations  for  the  KF  measurement  z  and  remain  constant  throughout  epoch  n  +  The 
associated  geolocation  error  values  {6xpc^  and  6ypc^)  for  the  new  ground  feature  have  a 
mean  of  zero  and  a  covariance  which  is  calculated  using  the  matrix  M. 

The  matrix  M  is  derived  from  the  measurement  equations  (2.22)  and  (2.23)  that 
follow: 

Xpc  Xc  Zci^Xffti  0c(  1  ■  Xfffi  •  y ftn  "  y/m^  ~ 

-  6x  -  6z‘  Xf  -  5<p  •  Xf  •  yf  +  66{\  +  Xj)  +  di/f  •  +  6xpc  -  5xf 

y pc  yc  Zci^fm  ■  Xfffi'y fm  “I"  0c(l  y fm)  "  -^/m^  — 

-  6y  -  6z-yf  -  5(f>(\  +  y^)  +  66  ■  Xf  ■  yf  -  6tff  ■  Xf  +  6ypc  -  6yf 
The  KF  measurement  equation  in  state  space  form  is  as  follows: 

z  =  H  •  dx  +  v 

where  the  vector  z  contains  the  input  measurement  values  from  the  camera.  These 
measurement  values  are  calculated  with  the  LHS  of  (2.22)  and  (2.23),  so,  the  input 
measurements  Z2  for  the  second  ground  feature  are  as  follows: 

Xc  Zc{xfffi2  ^c(l  ^ fm-i)  ’  y f mi  ’  y/mi^ 

yc  Zci^y fmi  ^c  '  ^fmi  '  y fmi  ^c(l  3^ '  ^fmi^  ^  ^ 

The  new  geolocated  measurements  (Xpc  and  ypc)  are  derived  in  (3.62)  for  epoch  n  +  1.  The 
associated  variance  of  these  measurements  is  extracted  from  the  RHS  of  (2.22)  and  (2.23). 
The  RHS  contains  all  the  error  terms  for  the  measurement  equation  and  accounts  for  the 
deviation  from  the  true  measurements  of  the  LHS.  This  is  equal  to  the  RHS  of  the  state 
space  measurement  equation,  therefore  the  following  is  true: 

-6x  -  6z  ■  Xf  -  6(f>  ■  Xf  yf  +  69(1  +  xj.)  +  6i//  ■  yf  +  6xpc  -  6xf 

H  dx  +  v  = 

-6y  -  6z-yf  -  6(f>(l  +  yj-)  +  69  ■  Xf  ■  yf  -  6tf/  ■  Xf  +  6ypc  -  6yf 
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where  the  KF  estimates  of  the  measurement  error  are  contained  in  6x.  This  is  better 
presented  as  follows: 

-6x  -  6z  •  Xf  -  6(p  ■  Xf  ■  jf  +  66(1  +  xj)  +  6if/  -  yf  +  5xpc  -  6xf 

H  dx  +  V  = 

-6y  -  6z-yf-  6^(1  +  yj)  +  66  ■  Xf  yf  -  6if/  ■  Xf  +  6yp^ 

where  v  accounts  for  the  variance  of  the  camera  measurements  in  the  focal  plane  (6xf  and 
6yf)  and  has  a  covariance  of  R.  The  KF’s  covariance  P  accounts  for  the  remaining  terms. 
Therefore,  the  covariance  of  the  geolocation  error  term  (6xpc  and  can  be  isolated 
from  the  navigation  error  state  terms  as  follows: 

=  6x  +  6z  ■  Xf  +  6^  ■  Xf  y f  -  66(\  +  x^r)  -  6ij/  -  yy  +  6xf 

_  _  (3.63) 

=  6y  +  6z  •  y f  +  d0(l  +  yj^)  -  66  ■  xy  ■  yy  +  6if/  ■  xy  +  6yy 

Recall  the  following: 

—  _ _ 

dx  =  6x,  6y,  6z,  6vjc,  6vy,  dv^.,  6(p,  66,  6\f/ 


Furthermore,  the  matrix  M  is  extracted  from  (3.63)  and  is  the  inverse  of  the  observation 
matrix  H  as  follows: 

1  0  .T/  0  0  0  xy-yy  -(l+.r^)  -yy 

0  1  0  0  0  (1  +Jp  ~Xy-yy  Xy 

where  xy  and  y/  measurements  are  for  the  new  geolocated  ground  feature  at  d  =  0  in 
epoch  n+  Therefore,  xy  =  2,  and  =  0  and  matrix  M  is  as  follows: 

1020000  -5  0 

M  =  (3.64) 

0  1  0  0  0  0  1  0  2 

L  -“2x9 

where 

6Xn(^  6Xf 

=  M-dx+  (3.65) 

Sypc  \  [  6yy 
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The  matrix  M  is  used  to  calculate  the  covariance  of  the  new  geolocated  ground  feature 
measurements,  which  is  then  combined  with  the  KF  covariance  matrix  P  during  the 
transition  between  epochs.  The  following  subsections  present  the  transition  of  the 
navigation  state  error  terms  and  the  associated  covariance  for  each  of  the  three  INS  system 
error  models. 

3.9.1  Transitioning  from  epoch  n  to  epoch  n  +  1  with  INS  Drift  Errors  Only. 

This  section  develops  the  transition  for  the  INS  system  with  only  random  drift  errors. 

/  — +\« 

The  KF’s  optimal  estimate  of  the  navigation  state  error  j  at  the  end  of  epoch  n  is 
transitioned  to  epoch  n  +  \  according  to  the  following: 


^*9X1 

^*9X1 

Sx 

Ay 

^•^pC2 

/  — +\n+l 

^  )  = 

^ypc2 

Sx 

^’^pC2 

0 

13x1 

0 

(3.66) 


where  the  nine  error  state  estimates  remain  the  same.  The  geolocation  error  estimates  for 
the  first  ground  feature  {5xpc^  and  in  epoch  n  are  discarded  because  the  ground 
feature  is  no  longer  in  the  camera’s  FOV.  However,  the  geolocation  error  estimates  for  the 
second  ground  feature  (dxpc^  and  epoch  n  are  transitioned  to  the  first  ground 

feature  position  for  epoch  n  +  1.  The  newly  acquired  ground  feature  error  estimates  are  set 
equal  to  zero  because  they  are  on  average  zero  with  a  covariance  derived  from  the  previous 
epoch’s  navigation  state  error  covariance  plus  the  camera  measurement  error  covariance. 
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The  covariance  j  of  the  navigation  state  error  estimates  for  the  last  camera 
measurement  L  at  the  end  of  epoch  n  is  as  follows: 


(\^ 


F(6x) 

^  >'9x9 

•  • 

•  • 

• 

P[6xp,^)  • 

•  • 
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•  P(^ypci) 

•  • 
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•  • 

P[dXpc^)  • 

• 

•  • 

•  P(sypc2) 

(3.67) 


Jl3xl3 

where  the  (•)  are  place-holders  representing  the  covariance  of  the  error  of  the  navigation 
state  error  estimate  for  each  component  of  the  navigation  state  error.  To  facilitate  the 
discussion  of  the  covariance  matrix  transition,  the  covariance  matrix  is  separated  into 

(\  77+1 

'o 

for  time  7’  =  0  in  epoch  n  -l-  1  is  separated  into  individual  blocks  and  labeled  as  follows: 
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The  block  labeled  Pn  is  a  9  x  9  matrix  that  contains  all  of  the  covariance  terms  for  the 
navigation  state  error  estimates.  The  block  labeled  Pi  2  is  a  9  x  2  matrix  which  contains 
the  off-diagonal  covariance  terms  corresponding  to  the  geolocation  error  estimates  for  the 
first  ground  feature.  The  block  labeled  P2,2  is  a  2  x  2  matrix  which  contains  the  diagonal 
covariance  terms  for  the  geolocation  error  estimates  for  the  first  ground  feature.  The  block 
labeled  Pi  3  is  a  9  x  2  matrix  and  the  section  labeled  P23  is  a  2  x  2  matrix.  These  two 
matrices  together  contain  the  off-diagonal  covariance  terms  corresponding  to  the 
geolocation  error  estimates  for  the  second  ground  feature.  The  block  labeled  P3  3  is  a  2  x  2 
matrix  that  contains  the  diagonal  covariance  terms  corresponding  to  the  geolocaton  error 
estimates  for  the  second  ground  feature. 
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Since  the  nine  navigation  state  error  estimates  remain  the  same  for  the  transition 
between  measurement  epochs,  the  corresponding  covariance  terms  will  remain  the  same 
as  well.  This  is  shown  as  follows: 


The  section  labeled  Pi, 2  is  populated  with  the  off-diagonal  covariance  terms  for  the 
previously  tracked  second  ground  feature.  This  is  presented  as  follows: 


The  terms  in  P23  in  epoch  n  are  discarded  because  they  correlate  the  covariance  of  the 
previously  tracked  first  ground  feature  with  the  covariance  of  the  second.  The  section 
labeled  P2,2  is  populated  with  the  diagonal  covariance  terms  for  the  previously  tracked 
second  ground  feature.  This  is  presented  as  follows: 
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The  covariance  terms  for  the  newly  tracked  ground  feature  in  sections  P13,  P23,  and  P33 
are  determined  by  using  matrix  M.  The  general  matrix  M  must  be  augmented  with  zeros 
to  account  for  the  geolocation  error  terms  in  dXj.  This  augmentation  is  shown  as  follows: 
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(3.69) 


The  covariance  terms  in  P13  are  derived  from  the  expectation  of  the  navigation  state 
error  terms  minus  the  KF  estimated  navigation  state  error  terms.  These  covariance  terms 
are  expressed  as  follows: 
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p 


n+1 


1.3n 


The  result  is  a  13  x  2  matrix,  where  the  top  nine  rows  fill  the  section  Pj  3.  The  covariance 
terms  in  P23  are  derived  from  the  covariance  of  the  previous  5xpc^  and  5ypc^  terms  in  rows 
12  and  13  of  the  covariance  matrix  minus  the  KF  estimated  values  for  Sxpc^  and  6ypc^. 
These  terms  are  shown  as  follows: 
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The  resulting  matrix  is  2  x  2,  which  fills  Pi.s  and  relates  the  previous  6xpc2  and  dypc^ 
uncertainty  with  the  new  measurement  uncertainty.  The  covariance  terms  in  P3  3  are 
derived  from  the  previous  covariance  plus  the  covariance  from  the  camera  measurement. 
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The  resulting  matrix  is  2  x  2,  which  fills  P3  3  and  is  the  covariance  for  the  geolocation  of 
the  new  ground  feature. 

3.9.2  Transitioning  from  epoch  n  to  epoch  n  +  \  with  INS  Bias  Errors  Only. 

This  section  develops  the  transition  for  the  INS  system  with  only  sensor  bias  errors. 
It  is  very  similar  to  Section  3.9.1,  except  that  bias  terms  must  be  accounted  for 
appropriately.  The  KF’s  optimal  estimate  of  the  navigation  state  error  at  the  end  of  epoch 
n  is  transitioned  to  epoch  n  +  1  according  to  the  following: 
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where  the  nine  error  state  estimates  and  the  six  INS  bias  error  estimates  remain  the  same. 


The  geolocation  error  estimates  for  the  first  ground  feature  {6xpc^  and  )  in  epoch  n  are 
discarded  because  the  ground  feature  is  no  longer  in  the  camera’s  FOV.  However,  the 
geolocation  error  estimates  for  the  second  ground  feature  {6xpc^  and  epoch  n  are 

transitioned  to  the  first  ground  feature  position  for  epoch  n+  The  newly  acquired 
ground  feature  error  estimates  {5xpc^^^  and  are  on  average  zero  with  a  covariance 

derived  from  the  previous  epoch’s  navigation  state  error  covariance  plus  the  camera 
measurement  error  covariance. 

The  KF’s  covariance  j  of  the  navigation  state  error  estimates  from  the  last 
measurement  L  at  the  end  of  epoch  n  is  as  follows: 
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where  the  (•)  are  place-holders  representing  the  covariance  values  of  the  error  of  the 
navigation  state  error  estimate  for  each  component  of  the  navigation  state  at  time  L  in 
epoch  n.  In  the  same  fashion  as  described  in  the  previous  section,  the  covariance  matrix  is 
separated  into  smaller  sections  according  to  the  lines  in  (3.71).  Likewise  the  covariance 

/  \rt+l 

matrix  (P^l  for  time  d  =  0  in  epoch  n  -l-  1  is  separated  into  smaller  sections  and  labeled 
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according  to  the  following: 
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In  general,  the  transition  of  this  covariance  matrix  is  the  same  as  the  Free  INS  with  only 
drift  error  terms  shown  in  Section  3.9.1.  The  major  difference  is  the  inclusion  of  the  bias 
covariance  terms;  however,  their  covariance  does  not  change  between  measurement 
epochs  and  as  a  result  the  15x15  section  labeled  Pi,i  remains  the  same  through  the 
transition.  This  is  presented  as  follows: 


The  covariance  terms  associated  with  the  previously  tracked  first  ground  feature  are 
discarded  and  the  previously  tracked  second  ground  feature  terms  are  transitioned  into  the 
sections  for  the  first  ground  feature  covariance  terms,  presented  as  follows: 
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where  section  Pi  2  is  a  15  x  2  matrix  of  the  off-diagonal  terms  for  the  first  ground  feature 
and  section  P2,2  is  a  2  x  2  matrix  of  the  diagonal  terms  for  the  first  ground  feature.  Section 
P3  2  contains  the  correlated  covariance  terms  between  the  first  and  second  ground  features, 
therefore  it  is  discarded  from  measurement  L  of  epoch  n. 

The  covariance  terms  for  the  newly  tracked  ground  feature  in  sections  P13,  P2,3,  and 
P3  3  are  determined  by  using  matrix  M.  The  general  matrix  M  must  be  augmented  with 
zeros  to  account  for  the  bias  error  terms  and  the  geolocation  error  terms  in  dxas.  This 
augmentation  is  shown  as  follows: 
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The  elements  in  the  15x2  matrix,  labeled  Pi  3,  are  the  covariance  of  the  cross-correlated 
nine  navigation  state  error  estimates  and  six  bias  estimates  with  the  newly  geolocated 
ground  feature  measurement  estimates.  These  covariance  values  are  derived  from  the 
expectation  of  the  KF’s  previous  navigation  state  error  estimates  with  the  measurement 
variance  of  the  second  ground  feature.  These  covariance  values  are  as  follows: 
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The  result  is  a  19  x  2  matrix,  where  the  top  15  rows  fill  section  Pi  3.  The  covariance  terms 
in  P2,3  are  derived  from  the  covariance  of  the  previous  6xpc2  and  dy^cj  terms,  in  rows  18 
and  19  of  the  covariance  matrix.  P^V  is  expressed  as  follows: 
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The  resulting  matrix  is  2  x  2,  which  fills  P2,3  and  relates  the  previous  dxpc^  and  Sjpc^ 
uncertainty  with  the  new  measurement  uncertainty.  The  covariance  terms  in  P3  3  are 
derived  from  the  expectation  of  the  newly  geolocated  ground  feature  measurements,  and 
are  expressed  as  follows: 


P”;3;  =Mas-(P^)”-Mas'+l2-fr2 

The  resulting  matrix  is  2  x  2,  which  fills  P3  3  and  is  the  covariance  for  the  geolocation  of 
the  new  ground  feature. 

3.9.3  Transitioning  from  epoch  n  to  epoch  n  +  \  with  Both  INS  Error  Sources. 
The  transition  from  epoch  n  to  epoch  n  +  I  for  the  INS  system  with  both  random  drift 
errors  and  sensor  bias  errors  is  accomplished  in  the  same  manner  as  Section  3.9.2.  This  is 
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the  case  because  the  transition  only  changes  the  KF  navigation  state  error  estimate  and  the 
associated  covariance.  At  the  point  of  transitioning  between  epochs  the  KF  has  already 
accounted  for  the  uncertainty  in  the  navigation  state  error  due  to  system  process  noise. 
Therefore,  the  transition  of  the  geolocation  error  estimates  and  the  associated  covariance 
are  processed  in  the  same  manner  as  with  no  additive  system  process  noise. 

3.10  Summary 

In  summary,  this  chapter  has  fully  developed  the  navigation  scenario  for  simulation 
of  the  automated  driftmeter  fused  with  inertial  measurements.  Three  INS  sensor  error 
configurations  were  presented:  INS  drift  errors  exclusively,  INS  bias  errors  exclusively, 
and  combined  INS  errors.  The  Kalman  filter  fused  vision  measurements  from  the 
automated  driftmeter  with  INS  measurements  in  a  SLAM  process.  Chapter  4  will  now 
provide  the  results  for  these  simulations. 
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IV.  Results 


This  chapter  presents  the  simulation  results  of  the  automated  driftmeter  aided  INS 
developed  in  Chapter  3.  Monte  Carlo  analyses  are  applied  to  the  navigation  scenarios  to 
provide  a  statistical  representation  of  true  navigation  state  performance.  The  automated 
driftmeter  is  first  evaluated  with  the  same  camera  resolution  used  in  the  previous  works  of 
Relyea  [3]  and  Quarmyne  [4].  Two  levels  of  analysis  (60  or  a  500  run  Monte  Carlo)  are 
utilized.  A  60  run  Monte  Carlo  illustrates  the  performance  of  the  same  three  error 
parameters  presented  in  Chapter  3:  INS  drift  errors  exclusively,  INS  bias  errors 
exclusively,  and  combined  INS  errors.  Finally,  the  resolution  is  modified  to  provide 
improved  results  and  is  analyzed  with  a  500  run  Monte  Carlo,  presenting  a  comparison 
between  the  results  of  a  9  MP  and  25  MP  camera  in  tabular  form. 

The  resolution  of  the  camera  determines  its  measurement  standard  deviation.  It  is 
assumed  that  camera  measurements  would  have  an  uncertainty  of  one  pixel  relative  to  the 
truth  data.  The  standard  deviation  for  each  camera  specification  is  calculated  using 
equation  (3.42),  presented  again  here  for  convenience: 

Cc  =  \l -  •  10“^ 

V  #MP 

where  the  covariance  of  the  camera  measurement  is  the  inverse  of  the  number  of  pixels 
populating  the  focal  plane.  The  square  root  of  that  value  provides  the  standard  deviation 
of  those  camera  measurements. 

The  1-cr  inertial  sensor  specifications  for  this  analysis  are  drawn  from  Section  3.5 
and  presented  in  Table  4.1.  The  combined  specifications  provide  a  variance  of  1 
However,  some  sections  only  utilize  part  of  these  specifications  and  consequently  do  not 
provide  the  full  1  variance.  The  sampling  rate  of  the  inertial  sensors  and  the  camera  are 
set  to  1  Hz.  The  Kalman  filter  measurement  updates  from  the  camera  therefore  occur  at 
the  same  rate  that  inertial  measurements  are  made. 
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Table  4.1:  Calculated  1-cr  inertial  sensor  specifications  provided  a  variance  of 
1  ^  drift  error  due  to  the  influence  of  drift  and  biases.  These  values  are  non- 

nr 

dimensional  for  this  navigation  scenario. 


Parameter 

UNITS 

I  km/hr 

Sampling  interval 

s 

I 

Gyro  bias  sigma 

Non-Dim 

6.4247  X  10“^ 

Angular  random  walk 

Non-Dim 

2.8746  X  10-^ 

Accel  bias  sigma 

Non-Dim 

7.7II8X  10-6 

Velocity  random  walk 

Non-Dim 

4.0085  X  10-6 

4.1  Automated  Driftmeter  Utilizing  9  MP  Camera 

This  section  presents  the  results  of  the  three  Free  INS  error  parameters  presented  in 
Chapter  3.  Section  4.1.1  presents  resutls  for  INS  drift  errors  only.  Section  4.1.3  presents 
the  results  for  INS  bias  errors  only.  Section  4.1.5  presents  the  results  for  a  combination  of 
the  INS  errors. 

4.1.1  A  Single  Run  with  INS  Drift  Errors  Only. 

The  KF  is  able  to  estimate  the  Free  INS’s  drift  error,  as  illustrated  in 
Figures  4.1  and  4.2.  The  KF’s  covariance  indicates  the  uncertainty  of  the  estimate,  while 
the  Free  INS’s  covariance  indicates  the  uncertainty  in  position.  Due  to  the  error 
accumulations  within  15  epochs  (150  seconds)  the  Free  INS’s  uncertainty  in  position  is 
5  m.  However,  the  Free  INS’s  uncertainty  in  position  accumulates  to  approximately  600  m 
within  360  epochs  (1  hour).  The  KF’s  covariance  for  the  x-position  also  accumulates  with 
time,  while  the  covariance  of  the  y  &  z-positions  stabilize.  This  indicates  the  KF’s 
confidence  in  its  estimate. 
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Figure  4.1:  First  15  epochs  of  position  error  with  only  INS  drift  errors,  aided 
by  a  9  MP  camera.  The  Free  INS  position  error  is  indicated  with  a  red  solid 
line  and  the  KF  estimate  with  a  blue  solid  line.  The  Free  INS  covariance  is 
indicated  with  a  black  dashed  line  and  the  KF  covariance  is  indicated  with  a 
green  dashed  line. 


Figure  4.2:  Full  360  epochs  of  position  error  with  only  INS  sensor  drift  errors, 
aided  by  a  9  MP  camera.  Color  scheme  is  the  same  as  that  in  Figure  4.1. 
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4.1.2  60  Run  Monte  Carlo  Analysis  with  INS  Drift  Errors  Only. 

The  60  run  Monte  Carlo  analysis  provides  a  statistical  representation  of  the  Free  INS 
and  the  aided  INS  performance,  and  is  indicated  in  Figures  4.3,  4.4,  and  4.5.  This  analysis 
includes  each  individual  run  of  the  Free  INS’s  and  the  aided  INS’s  position  error,  along 
with  their  mean  and  standard  deviations.  The  mean  values  are  provided  to  illustrate  the 
quality  of  the  Monte  Carlo  analysis,  which  is  determined  by  comparing  the  mean  system 
output  to  the  mean  of  the  Gaussian  distribution  input  (zero  for  this  data). 

In  Figure  4.3,  the  x-axis  standard  deviation  of  the  aided  INS  performance  is  the  same 
as,  if  not  worse  than,  the  Free  INS  performance.  Along  the  y-axis  (in  Figure  4.4)  the  aided 
system’s  performance  is  significantly  better  than  the  Free  INS’s  performance,  such  that  the 
standard  deviation  constrains  its  mean  error  close  to  zero.  Likewise,  the  aided  system’s 
performance  along  the  z-axis,  (in  Figure  4.5)  is  even  better  than  the  y-position’s 
performance,  such  that  the  standard  deviation  constrains  its  mean  error  closer  to  zero. 
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Figure  4.3:  60-run  Monte  Carlo  of  x-position  error  with  only  INS  sensor  drift 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 


80 


Aided  INS  Error  Mean 
Free  INS  Error  Mean 
Aided  INS  Std  dev  1o 
Free  INS  Std  Dev  1a 
Aided  INS  y-pos. 

Free  INS  y-pos. 
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Figure  4.4:  60-run  Monte  Carlo  of  y-position  error  with  only  INS  sensor  drift 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 
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Figure  4.5:  60-run  Monte  Carlo  of  z-position  error  with  only  INS  sensor  drift 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 
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The  final  navigation  state  error  standard  deviation  results  from  this  60  run  Monte 


Carlo  analysis  are  presented  in  Table  4.2. 


Table  4.2:  60  Run  Monte  Carlo  with  only  drift  final  navigation  state  error 
standard  deviations. 


Std  Dev 

Unaided  Final  Value 

Aided  Final  Value 

Percent  Improved 

(Tx 

0.694  km 

899.93  m 

-29.7% 

CTy 

0.751  km 

20.90  m 

97.2% 

O-z 

0.576  km 

4.79  m 

99.2% 

4.30  X  10~^  m/s 

4.81  X  10-5  m/s 

-11.9% 

4.73  X  10“^  m/s 

1.72  X  10-^  m/s 

96.4% 

5.88  X  10-2  m/s 

2.54  X  10-^  m/s 

99.6% 

1.73  X  10-5  rad 

1.13  X  10-5  rad 

34.7% 

O-Q 

1.76  X  10-5  rad 

1.49  X  10-5  rad 

15.3% 

O' 

1.85  X  10-5  rad 

5.91  X  10-5  rad 

-219.5% 

4.1.3  A  Single  Run  with  INS  Bias  Errors  Only. 

The  KF’s  ability  to  estimate  the  Free  INS’s  bias  error  is  illustrated  in 
Figures  4.6  and  4.7.  Contrary  to  the  drift  error  analysis,  this  Free  INS’s  error  accumulates 
to  1  m  within  the  first  15  epochs  (150  seconds).  For  360  epochs  (1  hour),  the  Free  INS’s 
uncertainty  in  position  accumulates  to  approximately  600  m,  similar  to  the  drift  error  plot. 
This  KF’s  covariance  for  the  x-position  also  increases  with  time;  however,  remains  less 
than  the  covariance  of  the  drift  error-faulted  INS.  This  KF’s  covariance  for  the  y  & 
z-positions  stabilize  in  the  same  way  as  the  system  that  only  contains  drift  errors. 
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Figure  4.6:  First  15  epochs  of  position  error  with  only  INS  bias  errors,  aided 
by  a  9  MP  camera.  The  Free  INS  position  error  is  indicated  with  a  red  solid 
line  and  the  KF  estimate  with  a  blue  solid  line.  The  Free  INS  covariance  is 
indicated  with  a  black  dashed  line  and  the  KF  covariance  is  indicated  with  a 
green  dashed  line. 
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Figure  4.7:  Full  360  epochs  of  position  error  with  only  INS  sensor  bias  errors, 
aided  by  a  9  MP  camera.  Color  scheme  is  the  same  as  that  in  Fig  4.6. 
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4.1.4  60  Run  Monte  Carlo  Analysis  with  INS  Bias  Errors  Only. 

This  section  presents  the  results  of  a  60  run  Monte  Carlo  analysis  with  only  INS 
sensor  bias  errors  in  Figures  4.8,  4.9,  and  4.10.  Contrary  to  the  drift  error  analysis,  when 
the  standard  deviation  of  the  aided  INS  performance  along  the  x-axis  is  evaluated,it  proves 
slightly  better  than  the  Free  INS  performance.  In  the  same  manner,  the  aided  INS 
performance  along  the  y  and  z  axes  is  much  better  than  the  Free  INS  performance. 


Aided  INS  Error  Mean 
Free  INS  Error  Mean 
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Figure  4.8:  60-run  Monte  Carlo  of  x-position  error  with  only  INS  sensor  bias 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 

Throughout  these  simulations,  spikes  are  apparent  from  white  Gaussian  noise  caused  by 
the  camera  measurement  noise.  They  are  more  evident  early  in  the  simulations  due  to  the 
smaller  average  values  of  the  estimates.  These  spikes  in  the  standard  deviations  caused 
unstable  results  at  times,  which  are  not  presented  in  the  results  section,  because  they  are 
considered  statistical  outliers. 
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•  Aided  INS  Error  Mean 

■  Free  INS  Error  Mean 

■  Aided  INS  Std  dev  1o 

■  Free  INS  Std  Dev  1a 
Aided  INS  y-pos. 

Free  INS  y-pos. 
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Figure  4.9:  60-run  Monte  Carlo  of  y-position  error  with  only  INS  sensor  bias 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 
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Figure  4.10:  60-run  Monte  Carlo  of  z-position  error  with  only  INS  sensor  bias 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 
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The  final  navigation  state  error  standard  deviation  results  from  this  60  run  Monte 


Carlo  analysis  are  presented  in  Table  4.3. 


Table  4.3:  60  Run  Monte  Carlo  with  only  bias  final  navigation  state  error 
standard  deviations. 


Std  Dev 

Unaided  Final  Value 

Aided  Final  Value 

Percent  Improved 

(Tx 

0.738  km 

369.01  m 

50.0% 

CTy 

0.700  km 

9.56  m 

98.6% 

O-z 

0.498  km 

2.54  m 

99.49% 

5.41  X  10-3  m/s 

2.48  X  10-3  m/s 

54.2% 

4.92  X  10-3  m/s 

3.68  X  10-3  m/s 

99.25% 

5.40  X  10-3  m/s 

1.32  X  10-3  m/s 

99.8% 

2.31  X  10-3  rad 

8.13  X  10-^  rad 

64.8% 

O-Q 

2.64  X  10-3  rad 

1.25  X  10-3  rad 

52.7% 

O' 

2.26  X  10-3  rad 

3.66  X  10-3  rad 

-61.9% 

4.1.5  A  Single  Run  with  Combined  INS  Sensor  Errors. 

The  KF’s  ability  to  estimate  both  the  drift  and  bias  errors  combined  in  the  Free  INS  is 
illustrated  in  Figures  4.11  and  4.12.  From  this  single  run,  significant  change  in 
performance  is  not  evident  when  compared  to  just  one  type  of  inertial  sensor  error 
simulation.  In  the  x-axis,  the  KF’s  covariance  is  slightly  better  than  the  Free  INS’s 
covariance.  However,  in  the  y  and  z-axis,  the  KF  covariance  is  significantly  better  than  the 
Free  INS  covariance. 
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Figure  4.1 1:  First  15  epochs  of  position  error  with  combined  INS  errors,  aided 
by  a  9  MP  camera.  The  Free  INS  position  error  is  indicated  with  a  red  solid 
line  and  the  KF  estimate  with  a  blue  solid  line.  The  Free  INS  covariance  is 
indicated  with  a  black  dashed  line  and  the  KF  covariance  is  indicated  with  a 
green  dashed  line. 


Figure  4. 12:  Full  360  epochs  of  position  error  with  combined  INS  sensor  errors, 
aided  by  a  9  MP  camera.  Color  scheme  is  the  same  as  that  in  Fig  4.11. 
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4.1.6  60  Run  Monte  Carlo  Analysis  with  Combined  INS  Sensor  Errors. 

This  section  provides  the  results  of  a  60  run  Monte  Carlo  analysis  where  both  the 
drift  and  bias  errors  are  combined  in  the  Free  INS.  These  results  are  shown  in 
Figures  4.13,  4.14,  and  4.15. 

Having  great  similarity  to  the  simulations  with  just  drift  error  in  the  Free  INS,  the 
aided  INS  standard  deviation  is  slightly  better  than  the  Free  INS  standard  deviation  in  the 
X  direction.  The  aided  y  and  z  standard  deviation  of  the  position  error  remains 
significantly  better  than  the  Free  INS. 
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Figure  4.13:  60-run  Monte  Carlo  of  x-position  error  with  combined  INS  sensor 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 
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Figure  4.14:  60-run  Monte  Carlo  of  y-position  error  with  combined  INS  sensor 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 
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Figure  4.15:  60-run  Monte  Carlo  of  z-position  error  with  combined  INS  sensor 
errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 
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The  final  standard  deviation  of  the  navigation  state  error  from  this  60  run  Monte 


Carlo  analysis  is  presented  in  Table  4.4. 


Table  4.4:  60  Run  Monte  Carlo  with  combined  INS  sensor  errors  final 
navigation  state  error  standard  deviations. 


Std  Dev 

Unaided  Final  Value 

Aided  Final  Value 

Percent  Improved 

(Tx 

0.878  km 

1,091.21  m 

-24.2% 

CTy 

0.908  km 

85.03  m 

90.6% 

O-z 

0.736  km 

4.85  m 

99.3% 

6.09  X  10“^  m/s 

4.91  X  10-5  m/s 

19.4% 

5.90  X  10~^  m/s 

3.02  X  10-^  m/s 

94.9% 

1.88  X  10-^  m/s 

2.31  X  10-^  m/s 

99.9% 

2.46  X  10“^  rad 

1.39  X  10-5  rad 

43.5% 

O-Q 

2.63  X  10-5  rad 

1.65  X  10-5  rad 

37.3% 

O' 

2.47  X  10-5  rad 

2.43  X  10-^  rad 

-883.8% 

4.2  Comparison  of  a  9  MP  with  a  25  MP  Camera  Using  500  Run  Monte  Carlo 

The  results  from  the  previous  60  run  Monte  Carlo  analyses  are  sufficient  to  show  that 
the  system’s  performance  is  inadequate  with  the  9  MP  camera  and  requires  modification. 
This  section  performs  a  500  run  Monte  Carlo  analysis,  first  with  the  9  MP  camera  and 
then  with  a  modified  25  MP  camera.  The  error  configuration  utilizing  both  error  sources  is 
most  similar  to  a  real  Free  INS  system.  Therefore,  Section  4.2  will  only  be  concerned  with 
this  configuration. 

4.2.1  500  Run  Monte  Carlo  Utilizing  a  9  MP  camera. 

The  plots  for  the  500  run  Monte  Carlo  analysis,  using  a  9  MP  camera,  are  presented 
in  Figures  4.16,  4.17,  and  4.18. 
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Figure  4.16:  500  Run  Monte  Carlo  of  x-position  error  with  eombined  INS 
sensor  errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 


Figure  4.17:  500  Run  Monte  Carlo  of  y-position  error  with  eombined  INS 
sensor  errors  and  aided  by  an  automated  driftmeter,  with  a  9  megapixel  eamera. 
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Figure  4.18:  500  Run  Monte  Carlo  of  z-position  error  with  combined  INS 
sensor  errors,  aided  by  an  automated  driftmeter,  with  a  9  megapixel  camera. 

The  final  standard  deviation  of  the  navigation  state  error  from  this  500  run  Monte 
Carlo  analysis  is  presented  in  Table  4.5.  These  results  should  represent  a  stronger 
statistical  representation  for  the  9MP  combined  sensor  error  than  the  60  run  Monte  Carlo. 

Table  4.5:  Final  navigation  state  error  standard  deviation  from  a  500  Run  Monte 
Carlo  with  combined  INS  sensor  errors  and  a  9  MP  camera 


Std  Dev 

Unaided  Final  Value 

9  MP  Aided  Final  Value 

Percent  Improved 

CTx 

0.992  km 

1024.76  m 

-3.2% 

(Ty 

1.081  km 

125.18  m 

88.4% 

O-z 

0.698  km 

5.29  m 

99.2% 

6.72  X  10“^  m/s 

5.24  X  10-5  m/s 

22.0% 

7.13  X  10“^  m/s 

3.97  X  10-^  m/s 

94.4% 

3.73  X  10“^  m/s 

2.4  X  10-4  m/s 

99.4% 

cr 0 

2.95  X  10-5  rad 

1.39  X  10-5  rad 

52.9% 

O-Q 

2.94  X  10-5  rad 

1.85  X  10-5  rad 

37.1% 

O'  ifj 

2.97  X  10-5  rad 

3.53  X  10-4  rad 

-1088.5% 
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4.2.2  500  Run  Monte  Carlo  Utilizing  a  25  MP  camera. 

The  plots  for  the  500  run  Monte  Carlo  analysis,  using  a  25  MP  camera,  are  presented 
Figures  4.19,  4.20,  and  4.21. 


Epochs  [secxIO] 


Figure  4.19:  500-run  Monte  Carlo  of  x-position  error  with  combined  INS 
sensor  errors,  aided  by  an  automated  driftmeter,  with  a  25  megapixel  camera. 
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Figure  4.20:  500-run  Monte  Carlo  of  y-position  error  with  combined  INS 
sensor  errors,  aided  by  an  automated  driftmeter,  with  a  25  megapixel  camera. 
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Figure  4.21:  500-run  Monte  Carlo  of  z-position  error  with  eombined  INS 
sensor  errors,  aided  by  an  automated  driftmeter,  with  a  25  megapixel  eamera. 

The  final  standard  deviation  of  the  navigation  state  error  from  this  500  run  Monte 
Carlo  analysis  is  presented  in  Table  4.6.  By  simply  ehanging  the  eamera  resolution, 
signifieant  improvements  are  evident  throughout  these  tabulated  results. 


Table  4.6:  Final  navigation  state  error  standard  deviation  from  a  500  Run  Monte 
Carlo  with  eombined  INS  sensor  errors  and  a  25  MP  eamera 


Std  Dev 

Unaided  Final  Value 

25  MP  Aided  Final  Value 

Pereent  Improved 

CTx 

1.009  km 

856.22  m 

15.2% 

O-y 

988.01  km 

43.49  m 

95.6% 

0.730  km 

4.10m 

99.4% 

6.68  X  10-3  m/s 

4.11  X  10-3  m/s 

38.5% 

(Tv, 

6.57  X  10-3  m/s 

1.93  X  10-4  m/s 

97.1% 

^Vz 

5.88  X  10-2  m/s 

2.01  X  10-4  m/s 

99.7% 

O"  (f, 

2.97  X  10-3  rad 

1.39  X  10-3  rad 

53.2% 

ere 

2.90  X  10-3  rad 

1.52  X  10-3  rad 

47.6% 

O'  ijj 

2.80  X  10-3  rad 

1.24  X  10-4  rad 

-342.9% 
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4.3  Summary 

This  chapter  presents  the  simulation  results  of  the  automated  driftmeter  aided  INS 
developed  in  Chapter  3.  The  driftmeter  was  first  evaluated  with  a  9  MP  camera.  The 
results  from  a  single  run  of  each  of  the  three  Free  INS  error  parameters  illustrate  that  the 
KF  is  able  to  estimate  the  bias  error  of  the  Free  INS  more  easily  than  the  drift  error, 
providing  better  aided  navigation  performance.  It  is  also  noted  that  the  estimate  of  the 
x-position  error  is  very  weak  and  is  further  supported  with  60  run  Monte  Carlo  analyses, 
which  statistically  represents  the  performance  of  the  aided  system.  The  final  standard 
deviation  values  of  each  navigation  state  error  are  presented  in  tables  corresponding  to 
these  Monte  Carlo  analyses.  These  final  navigation  state  error  values  are  used  to 
determine  the  percent  improvement  of  the  aided  system.  From  these  results  it  is  noted  that 
the  aiding  for  the  if/  Euler  angle  is  also  very  week.  Finally,  the  camera  resolution  is 
modified  to  25  MP  and  the  two  different  systems  are  compared  using  a  500  run  Monte 
Carlo,  where  it  is  shown  that  25  MP  resolution  provides  improved  navigation  state  error 
estimation  and  aiding  throughout  all  nine  error  states. 
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V.  Conclusions  and  Recommendations 


This  thesis  advances  the  development  of  a  method  for  aiding  a  navigation-grade  INS 
with  an  optical  sensor,  acting  as  a  automated  driftmeter.  The  motivation  of  this  research  is 
to  achieve  precision  navigation  similar  to  GPS  and  thus  providing  an  alternative  passive 
navigation  system  for  military  aircraft  applications.  Simulations  are  performed  to  evaluate 
the  KF’s  ability  to  mitigate  the  deleterious  effects  of  the  inertial  sensor  errors  of  a  Free 
INS.  The  main  contribution  of  this  research  is  the  calculation  of  the  covariance  of  the 
geolocation  error  of  a  ground  feature  and  then  tracking  that  feature  until  it  disappears  from 
the  field  of  view  of  the  camera,  whereupon  it  is  replaced  by  a  newly  geolocated  ground 
feature.  Monte  Carlo  analyses  are  performed  in  order  to  gain  a  statistical  representation  of 
this  new  aided  system’s  performance.  This  chapter  summarizes  the  results  of  these  Monte 
Carlo  analyses  and  provides  recommendations  for  future  research. 

5.1  Conclusions 

This  research  incorporates  a  linear  Kalman  filter  algorithm,  which  utilizes  bearing 
measurements  from  geolocating  and  tracking  ground  features  to  aid  a  Free  INS  during  a 
simulated  one  hour  flight.  Two  levels  of  Monte  Carlo  analysis,  a  60  run  and  a  500  run,  are 
performed,  and  their  results  are  compiled  to  establish  the  degree  of  improvement  afforded 
by  this  method  of  aiding  a  navigation-grade  INS  with  an  optical  sensor. 

The  current  configuration  of  this  automated  driftmeter- aided  INS  does  not  meet  the 
precision  navigation  standards  established  by  GPS.  It  is  shown  with  the  single  run 
simulations  that  the  linear  KF  is  able  to  precisely  estimate  the  x,  y,  and  z-position  errors 
within  the  first  15  epochs  (that  is,  after  having  geolocated  15  ground  features  over  a 
timespan  of  150  s).  Over  a  longer  duration  of  time,  however,  the  linear  KF  is  not  able  to 
estimate  the  error  for  the  x-position  data  as  effectively.  In  the  best  case  scenario  with 
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combined  Free  INS  error  sources,  and  utilizing  the  25  MP  camera,  the  x-position  error 
range  is  reduced  to  856  m.  This  range  is  far  from  the  accuracy  of  GPS. 

5.2  Recommendations  for  Future  Work 

The  inability  to  estimate  the  errors  in  the  x-position  has  been  persistent  throughout 
this  research  as  well  as  the  previous  research  by  Quarmyne  [4]  and  Relyea  [3].  This 
problem  is  not  resolved  herein.  Any  future  work  should  focus  on  the  disparity  between 
position  error  measurements.  This  may  be  resolved  by  using  an  indirect  feedback  KF 
configuration,  as  discussed  in  Chapter  2. 

The  spikes  that  are  due  to  the  camera  measurement  noise  should  be  resolved  to 
improve  overall  reliability  of  the  aided  system.  This  may  be  done  by  implementing 
residual  monitoring  in  the  KF  algorithm.  This  is  done  to  establish  ’’reasonableness 
checking  of  measurements  before  they  are  processed  by  the  filter”  [1]. 

Further  research  should  evaluate  the  benefit  of  providing  precise  altitude  information 
to  the  KF  and  thus  reducing  the  coupled  position  error  effect  from  the  INS  in  the 
measurement  equation.  This  additional  altitude  information  should  improve  the  overall 
navigation  capabilities  of  this  autonomous  driftmeter.  After  all,  altitude  information  was 
used  with  driftmeter  measurements  in  the  golden  age  of  navigation. 

This  research  employs  a  simplified  navigation  scenario,  simulated  in  MATLAB,  to 
evaluate  the  possibilities  of  using  bearing-only  optical  measurements.  The  results  proved 
inferior  to  the  performance  of  the  precision  navigation  standard  of  GPS.  This  research 
does,  however,  have  the  potential  to  provide  an  avenue  for  future  improvement  of  this 
passive  navigation  tool  for  military  applications. 
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Appendix  A:  Honeywell  HG9900  INS 


Specifications  from  a  commercial  navigation  grade  INS  are  used  for  comparison  with 
the  calibrated  INS  sensor  values.  The  Honeywell  HG9900  is  a  navigation  grade  INS  with 
a  long-time  position  accuracy  of  0.8  ^  performance,  which  is  equivalent  to  1.48  The 
HG9900  IMU’s  1-cr  error  specifications  are  provided  in  Table  A.l. 

Table  A.l:  Honeywell  HG9900  tactical-grade  IMU  1-cr  error  specifications 
[22].  The  value  for  the  velocity  random  walk  is  not  provided. 


Parameter 

UNITS 

HG9900 

Sampling  interval 

ms 

3.33 

Gyro  bias  sigma 

deg/hr 

<  0.003 

Angular  random  walk 

deg/ 

<  0.002 

Gyro  scalefactor  sigma 

PPM 

<  5.0 

Accel  bias  sigma 

mjs^ 

<  25  X  10-6 

Velocity  random  walk 

m/s/ 

not  provided 

Accel  scalefactor  sigma 

PPM 

<  100 

The  gyro  scale  factor  causes  an  error  to  occur  only  during  actual  rotation.  The 
accelerometer  scale  factor  causes  an  error  to  occur  only  during  actual  acceleration.  They 
are  specified  in  terms  of  Parts  Per  Million  (PPM).  Therefore,  the  error  for  360  degrees  of 
rotation  is  computed  as  follows: 


\ 1000000 


•  360  deg  =  0.0018  deg 


The  error  for  an  acceleration  of  10  is  computed  as  follows: 


100  \ 
1000000 ) 


,  ^  m  _ m 

•  10  —  =  0.001  - 
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In  the  scenario  in  this  research,  the  nominal  rotation  and  acceleration  are  zero;  therefore, 
the  scale  factor  errors  can  be  ignored.  Section  A.l  analyzes  the  Gyro’s  error  dynamics  to 
better  understand  how  these  error  specifications  relate  to  the  calibrated  error  values. 


A.l  Connecting  the  HG9900  Gyro’s  Specifications  to  the  Mathematical  Model  of 
the  Error  Equations 

The  continuous-time  dynamics  are  expressed  as 

9  =  <ji>  +  b  +  ac-w,  00  =  0  (A.l) 


where  oj  is  the  true  input  angular  rate,  b  is  the  gyro’s  random  bias,  b  ~  A(0,  cr^)  and  w  is  a 
unit  Brownian  motion  w  ~  A(0,  t).  Therefore,  the  mathematical  model’s  units  for  the 
gyro’s  continuous-time  drift  parameter  cr^  are  ^  and  it  is  related  to  the  HG9900  Angle 

ys 

Random  Walk  specification  ere  as  follows: 


CTc 


CTc 


0-0  rad 


60  •  57.3 
0.002 
60  •  57.3 


=  5.817  X  10“’ 


rad 


The  discrete-time  system  dynamics  are  as  follows: 


(A.2) 

(A.3) 


0/t+i  -  9f:  +  (a>k  +  b)AT  -I- 


(A.4) 


where 

-  N{0,  ct])  ,  00  =  0  (A.5) 

When  the  true  angular  rate  cok  is  zero  and  the  bias  is  zero,  then  6k  ~  N{0,  k  ■  cr^)  (where 
k  =  3600  •  fs  at  one  hour).  Therefore  the  following  is  true: 

cr’  =  3600  ■  fs-cr]  (A.6) 


which  is 


o~e  1 
60  •  57.3  ■  ^ 
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The  sampling  frequency  is  1  //z  or  1  Therefore,  following  is  true: 


0.002  1 

(Td  =  — z — r  •  — =  =  5.82  X  10  '  rad 

60  •  57.3  VT 

The  manufacturer’s  specifications  have  now  been  related  to  the  parameters  of  the 
discrete-time  INS  update  scenario. 

A.2  Non-Dimensionalized  HG9900  Specifications 

The  HG9900’s  bias  and  drift  error  specifications  are  non-dimensionalized  according 
to  the  method  employed  in  Section  A.l  using  parameters  described  in  Section  3.1. 


Rate  Gyro  (HG9900) 

nnm  ^  ^  1000^ 

kf  51 3(iek  3600/  100^ 

/ 

nnm  ^  ^ 

•  0.002  •  — =  • - •  — 

513(kk  V36()^ 

Accelerometer  (HG9900) 

(ioop2 

•  not  provided 


(Tb^  =  1.45  X  10-’ 


ad^  =  5.82  X  10-" 


=  2.50  X  10“^ 


(Td„  ^  2.50  X  10  ^ 


The  accelerometer  random  walk  value  is  not  provided  in  the  manufacturer  specification 
sheet,  presumably  because  it  is  small  in  comparison  to  the  accelerometer  bias  and  scale 
factor  error.  However,  conversions  are  determined  for  the  remaining  1-cr  error  terms.  If  the 
accelerometer  drift  error  is  assumed  to  be  the  same  as  the  accelerometer  bias  error,  then 
the  average  of  the  HG9900  error  standard  deviations  is  1.3  x  10“^  compared  to  1.2  x  10“^ 
for  the  average  of  the  calibrated  INS  error  values  in  Table  3.3.  These  average  values  are 
closer  than  one  would  expect.  However,  the  assumption  that  accelerometer  random  walk 
value  is  equivalent  to  its  bias  value  is  probably  the  source  of  the  error  in  comparison  and  it 
can  be  justified  that  the  calibrated  INS  error  values  qualify  as  a  navigation  grade  INS. 
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Appendix  B:  Free  INS  Results 


B.l  Free  INS  with  Process  Noise  Errors  Only 

This  section  presents  the  results  from  the  free  INS  with  only  sensor  drift  errors. 


20  40  60  80  100  120  140  160  180  200  220  240  260  280  300  320  340  360 


20  40  60  80  100  120  140  160  180  200  220  240  260  280  300  320  340  360 


Epochs  [secxi  0] 

Figure  B.l:  Free  INS  position  error  caused  by  only  sensor  drift 
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Figure  B.2:  Free  INS  velocity  error  caused  by  only  sensor  drift 
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Figure  B.3:  Free  INS  attitude  error  caused  by  only  sensor  drift 

B.2  Free  INS  with  Bias  Errors  Only 

This  section  presents  the  results  from  the  free  INS  with  only  sensor  bias  errors. 


Epochs  [secx1 0] 

Figure  B.4:  Free  INS  position  error  caused  by  only  sensor  biases 
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Figure  B.5:  Free  INS  veloeity  error  eaused  by  only  sensor  biases 
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Figure  B.6:  Free  INS  attitude  error  eaused  by  only  sensor  biases 
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Figure  B.7:  Free  INS  accelerometer  bias 
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Figure  B.8:  Free  INS  gyroscope  bias 
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B.3  Free  INS  with  Combined  Sensor  Errors 


This  section  presents  the  results  from  the  free  INS  with  both  sensor  drift  and  bias 
errors. 
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Figure  B.9:  Free  INS  position  error  caused  by  both  sensor  drift  and  biases 
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Figure  B.IO:  Free  INS  velocity  error  caused  by  both  sensor  drift  and  biases 
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Figure  B.l  1:  Free  INS  attitude  error  eaused  by  both  sensor  drift  and  biases 
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Figure  B.12:  Free  INS  aeeelerometer  bias 
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Figure  B.13:  Free  INS  gyroscope  bias 
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